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1.  Introduction 


Many  studies  have  been  done  on  various  forms  of  mini-max  pursuit-evader  games  for  missiles 
and  aircraft.  These  types  of  algorithms  typically  give  performance  that  is  superior  to  simpler 
missile  guidance  algorithms  such  as  proportional,  navigation  or  linear  control  techniques.  The 
bibleography  gives  titles  and  abstracts  of  nine  papers  about  mini-max  pursuit-evader  algo¬ 
rithms,  as  well  as  one  paper  on  a  nonlinear  regulator  algorithm  that  could  be  useful  for  mis¬ 
sile  guidance. 

The  3D  mini-max  pursuit-evader  algorithms  typically  run  far  too  slow  for  real-time  implemen¬ 
tations,  so  they  are  restricted  to  simulation  use.  Since  they  are  run  off-line,  they  can  include 
highly  detailed  dynamics  and  constraints,  at  the  expense  of  run-time. 

In  this  report,  we  derive  a  3D  mini-max  pursuit-evader  algorithm  that  takes  between  250,000 
and  2  million  floating  point  operations  to  update.  This  guidance  algorithm  could  be  updated 
at  around  1  Hz  on  a  rather  slow  1  Mflop  computer.  In  order  to  get  this  performance,  we  had 
to  assume  that  both  vehicles  moved  at  (different)  piece-wise  constant  speeds,  along  piece-wise 
circular  3D  trajectories.  This  leaves  four  piece-wise  constant  control  inputs:  the  two  normal 
accelerations  of  each  of  the  vehicles.  Conventional  mini-max  algorithms  would  have  to 
search  through  a  four-dimensional  control  space  to  find  the  optimal  trajectories. 

With  the  assumptions  of  constant  speeds  and  constant  accelerations,  we  have  derived  the 
implicit  equations  determining  the  two-dimensional  surface  in  3D  on  which  all  possible  inter¬ 
cepts  occur.  The  two-dimensional  intercept  surface  typically  encloses  the  slower  aircraft, 
allowing  it  no  escape.  Fuel  and  acceleration  constraints  on  the  missile  leave  openings  in  the 
two-dimensional  intercept  surface  through  which  the  evading  aircraft  can  escape.  Our  gui¬ 
dance  algorithm  gets  its  speed  by  using  either  a  closed-form  solution  (2  million  floating  point 
operations)  or  an  iterative  solution  (250,(X)0  floating  point  operations)  for  all  points  on  a 
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30  X  60  grid  on  the  two-dimensional  surface  of  all  possible  intercept  points  in  3D  space. 

At  each  update  (e.g.  2  Hz),  the  algorithm  inputs  the  current  velocity  of  both  vehicles  and  the 
relative  position  between  them.  From  this  information,  the  missile  computes  what  accelera¬ 
tion  input  it  needs  to  nainimize  the  miss  distance  for  each  possible  aircraft  acceleration  input. 
This  results  in  a  2-dimensional  intercept  surface,  parameterized  by  the  aircraft’s  2-dimensional 
acceleration  input.  On  this  surface,  the  aircraft  computes  some  cost  functional  (e.g.  some 
combination  of  intercept  time,  max  missile  acceleration,  etc.)  that  it  wants  to  maximize  and 
choses  the  corresponding  aircraft  acceleration  input.  Then  the  missile  computes  the  accelera¬ 
tion  required  for  the  missile  to  get  to  that  point  on  the  intercept  surface. 

Given  the  circular  trajectory  from  the  missile’s  initial  position  to  the  desired  end-point,  with 
the  missile’s  irtitial  velocity  tangent  to  the  circle,  there  is  no  other  trajectory  that  simultane¬ 
ously  has  less  maximum-acceleration  and  less  flight  time.  We  ignore  paths  of  longer  length 
which  can  have  smaller  curvature.  By  using  the  smallest  reasonable  intercept  acceleration 
(circular  paths),  the  missile  avoids  its  own  acceleration  limits,  and  is  less  prone  to  hit  its 
angular  rate  limits  chasing  a  wildly  maneuvering  aircraft. 

A  disadvantage  of  the  proposed  algorithm  (compared  to  proportional  nav)  is  that  it  requires 
measurements  of  the  velocity  of  both  vehicles,  and  range.  Since  this  information  is  seldom 
available  from  sensors  on  a  small  missile,  the  missile  may  have  to  rely  on  getting  these  infor¬ 
mation  updates  from  the  aircraft  that  launched  the  missile.  Alternatively,  the  algorithm  could 
be  used  only  to  display  the  intercept  surface  to  the  pursuing  aircraft  to  let  the  pilot  know 
when  to  launch  a  missile.  Another  alternative  is  to  use  the  algorithm  on  the  evading  aircraft 
to  let  it  know  what  point  on  (or  hole  in)  the  intercept  surface  it  should  head  toward  to  best 
avoid  a  missile  that  has  been  launched  at  it. 

Included  near  the  end  of  this  report  are  some  examples  that  were  run  on  a  120  MHz  Pentium. 
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The  guidance  algorithm  itself  ran  at  approximately  80  Hz,  The  color  figures  (see  figures  10 
through  18)  are  snapshots  of  the  moving  graphics  done  using  OpenGL  which  Microsoft  has 
licensed  from  Silicon  Graphics  for  the  Windows  NT  operating  system. 
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2.  Equations  of  Motion  and  Mini-Max  Formulation 

Let  vehicle  1  be  the  evader,  and  vehicle  2  be  the  pursuer. 

Let  TjCt)  €  be  the  position  of  vehicle  i. 

Let  Vjft)  e  be  the  velocity  of  vehicle  i. 

Let  nii  be  the  mass  of  vehicle  i. 

Let  ^  €  Uj  c  R^  be  the  two  perpendicular  forces  on  vehicle  i. 

Let  Vj-i-(t)  be  the  orthonormal  (orthogonal)  complement  of  Vj(t). 


The  constant  speed,  point-mass  equations  of  motion,  in  inertial  coordinates,  are  given  by: 


miYi(t)  =  Vii-(t)Ui 

ri(t)  =  Vi(t)  i=l,2 


(1) 


with  initial  conditions: 


Vi(0)  5(0)  i=l,2 


(2) 


We  are  considering  mini-max  formulations  of  the  following  -type  for  determining  the  optimal 
control  inputs  Uj  =  Uj*.  The  pursuer  tries  to  minimize  the  miss  distance,  while  the  evader  tries 
to  minimize  some  cost  functional  J. 


[T*(ui),  U2*(ui)]  =  min_args 


min 

0^t<T^«  1112(0 -rj(t)|l 

526  U2CR^ 


miss  distance 


(3) 


Hi  =arg 


max 


UieUicR 


I  j[t*(ui),  U2*(Hi).  Ii(T*(ui)).  £2(T* (ui))j  ] 


cost  index  (4) 


U2*  =  U2*(Ui*) 


(5) 


The  mini-max  computation  is  updated  as  fast  as  the  guidance  computer  can  calculate  it,  using 
the  current  state,  ^(t),  Vi(t)  as  the  new  initial  conditions. 
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If  the  mini-max  algorithm  is  only  evaluated  once,  both  vehicles  will  have  constant  control 
inputs,  and  will  fly  along  circular  trajectories.  Each  time  the  mini-max  algorithm  is  updated, 
each  vehicle  recomputes  the  optimal  value  of  its  control  inputs.  This  results  in  each  vehicle’s 
trajectory  consisting  of  smoothly  connected  circular  segments. 

For  some  initial  conditions  1^(0),  VjCO),  large  enough  values  of  and  large  enough 

control-input  sets  Uj,  the  vehicles  intercept  (zero  miss  distance)  for  any  value  of 
Ui  6  Uj  c  R^,  i.e.: 

min 

Ill2(0-Ii(t)ll 

^6  U2CR^ 

In  this  case,  T*(^)  is  the  intercept  time,  and  rj(T*(ui))  =  r2(T*(ui))  is  the  intercept  point  If 
more  than  one  intercept  occurs,  T*(ui)  is  the  smallest  of  those  intercept  times.  The  function 
ri(T*(^))  maps  the  2-dimensional  control  space  Uj  into  R^,  giving  the  2-dimensional  inter¬ 
cept  surface  in  3-dimensional  space. 

Examples  of  the  evader’s  cost  functional  J  could  be: 

J  =  T* (uj)  maximize  intercept  time,  depleting  pursuer's  fuel 

J  =  ||^*(ui)l|  maximize  pursuer  acceleration,  putting  pursuer  on  acceleration  limits  (7) 
J  =  H^(T* (ui))  -  rj(T* (uj))!!  maximize  miss  distance  (if  non-zero) 
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Circular  Paths  have  Minimum  Max«Acceleration 

Given  the  unique  circular  trajectory  from  the  missile’s  initial  position  to  the  desired  end-point, 
with  the  missile’s  initial  velocity  tangent  to  the  circle,  there  is  no  other  trajectory  that  simul¬ 
taneously  has  less  maximum-acceleration  and  less  flight  time.  We  ignore  paths  of  longer 
length  which  can  have  smaller  curvature.  By  using  the  smallest  reasonable  intercept  accelera¬ 
tion  (circular  paths),  the  missile  avoids  its  own  acceleration  limits,  and  is  less  prone  to  hit  its 
angular  rate  limits  chasing  a  wildly  maneuvering  aircraft. 

For  a  proof  that  min-length  paths  with  a  local  curvature  bound  consist  of  a  circular  segment 
and  a  straight  segment,  see  [Dubins]  whose  work  was  also  sponsored  by  AFOSR. 


3.  Intercept  Surface  for  Vehicles  on  Constant-Speed  3D  Circular  Trajectories 


3.1  Introduction 

If  speed  and  acceleration  are  allowed  to  vary  within  some  bounded  region,  the  associated 
mini-max  evader-puTsuer  problem  is  computationally  expensive  and  unlikely  to  be  solved  in 
real-time  by  today’s  computers.  By  assuming  constant  speed  and  any  allowed  constant  nor¬ 
mal  acceleration,  the  problem  becomes  tractable.  In  later  sections,  we  will  partially  alleviate 
the  restrictions  of  constant  speed  and  constant  normal  acceleration,  by  recomputing  the  current 
optimum  "constant"  accelerations  using  the  current  positions  and  speeds,  as  the  trajectories 
evolve. 

In  this  section,  we  will  derive  the  equations  that  define  the  intercept  surface  for  two  vehicles 
on  constant-speed  circular  trajectories.  In  the  next  two  sections  we  will  give  algorithms  for 
solving  the  resulting  set  of  implicit  equations. 

Assume  the  evader  (aircraft)  has  initial  location  £^(0)  and  initial  velocity  Vi(0).  Assume  the 
pursuer  (missile)  has  initial  location  £2(0)  and  initial  velocity  V2(0). 

If  both  vehicles  maintain  constant  speed  and  constant  normal  acceleration,  they  both  will 
remain  on  circular  paths  in  3D  space.  The  set  of  constant-speed  circular  trajectories  through 
the  point  £2(0),  with  initial  velocity  V2(0)  is  2-dimensional,  and  could  be  parameterized  by  the 
two  components  of  normal  acceleration.  Similarly,  the  set  of  constant-speed  circular  trajec¬ 
tories  through  the  point  £^(0),  with  initial  velocity  ^(0)  is  2-dimensional,  and  could  be 
parameterized  by  the  two  components  of  normal  acceleration.  The  constraint  that  the  circles 
for  vehicle!  and  vehicle2  intersect  reduces  the  4-dimensional  set  to  a  3-dimensional  set.  The 
constraint  that  vehicle!  and  vehicle2  arrive  at  the  intersection  point  at  the  same  time,  reduces 

the  intercept  surface  to  a  2-dimensional  set.  Figure  1  shows  typical  intercepts. 
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:  Intercepts  Along  Circular  Paths 


This  two-dimensional  surface  separates  3D  space  into  regions  that  the  aircraft  can  get  to  first, 
and  regions  that  the  missile  can  get  to  first.  If  the  missile  is  capable  of  catching  the  aircraft, 
then  the  regions  that  the  aircraft  can  get  to  first  include  a  bounded  region  in  front  of  the  air¬ 
craft  that  shrinks  to  a  point  as  the  missile  approaches  intercept. 

To  solve  for  the  locations  £  in  where  the  vehicles  can  intercept,  we  will  compute  the  time 
it  takes  vehiclej  and  vehicle2  to  get  to  point  r  via  circular  paths  from  ijCO)  and  £2(0). 

We  will  define  two  sets  of  polar  coordinates,  based  at  the  points  rj(0)  and  £2(0). 


r  =  £j(0)  +  ri 


Yi(0) 
l|Vi(0)l|  ’ 


COS(Tli) 

sin(Tii)cos(Ci) 

sin(Tii)sin(Ci) 


=  £2(0)  +  r2 


Y2(0) 

IIY2(0)|| 


.  YjCO)-" 


COS(Tl2) 

sin(ii2)cos(C2) 

sin(Ti2)sin(C2) 


(8) 


where 


Yi(0) 
l|Vi(0)||  ’ 


Yi(O)-" 


e  SO(3) 


i=l,2 


(9) 


ri  =  ||r-£i(0)l|  r2  =  It -£2(0)11 


T|i  =  COS  ^ 


■  Vi(0) .  (r -£i(0))' 

■V2(0)  .  (£-£2(0))' 

llYi(0)I|  ri 

H2  —  CUo 

IIY2(0)11  r2 

From  equation  8,  we  also  see  that  Tij  is  the  angle  between  V|(0)  and  r  -  ij(0),  which  is  half 
the  angle  through  which  vehiclej  has  turned  while  on  the  circular  trajectory  from  £j(0)  to  £. 
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Let  Tj  be  the  time  it  takes  for  vehiclej  to  travel  along  the  circular  trajectory  from  rj(0)  to  r. 
The  circular  distance  that  vehicle}  travels  is  HVjCO)!!  Tj.  The  straight-line  chord  distance  from 


Tli 

rifO)  to  r  is  n.  The  ratio  of  circumferencial  distance  to  chord  distance  is  — : - where  11;  is 

-  sm(Tii) 

the  half-angle.  This  gives  us  formulas  for  computing  the  arrival  times  as  functions  of  rj  and 
Tli: 


ri  Til 

||Vi(0)||  sin(Tii) 


(10) 


T  =  ^2 

2  1|V2(0)||  sin(Tl2) 


(11) 


The  intercept  surface  is  then  obtained  by  solving  for  all  r  that  satisfy  the  equation:  T^  =  T2, 
i.e. 


ri  ^  ||Vi(0)I|  112  sindli) 

r2  l|V2(0)||  sin(ii2)  Til 


Equation  12  is  a  single  equation  in  the  three  variables:  (ri/r2,Tii,T|2).  Equations  8  and  12  give 
four  scalar  equations  on  the  six  variables  (ri,Tii,^i),  and  (r2,T|2,^2)-  ^  either  case  there  are 
two  more  variables  than  equations,  so  we  get  a  two-dimensional  solution  set. 

If  we  let: 

l|Vi(0)ll  TI2  sindli) 

11X2(0)11  sin(ii2)  Til  ^  ^ 

then  equation  12  can  be  written  as  0  =  F(r)  where: 

F(r)  =  ri-kr2  (14) 
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3.2  ParameteriTation  of  the  Intercept  Surface 


The  data  that  describes  the  intercept  surface  can  be  represented  as  two  vectors  in  a  homogene¬ 


ous  space: 

’Vi(O) 

datai=  £^(0)  i=l,2 

[  1 


(15) 


If  we  apply  a  Euclidean  transformation  that  translates  the  origin  to  £2(0),  rotates  the  V2(0) 
vector  to  the  x  axis,  and  rotates  about  this  new  x  axis  until  the  £i(0)  -  £2(0)  vector  has  no  z 
component,  then  the  data  becomes: 

(^3x3  ^3x1 

=  ^x3  -R‘^I2(0)  datai 

0lx3  0ix3  1 

R'^ViCO)' 
ri2Cos(los) 

=  ri2sin(los)  for  i=l  and 
0 
1 

where  los  is  the  angle  between  the  unit  vector  1_V2  ^  Y2((^)  direction  and  the  iri2  unit  vec¬ 

tor  in  the  £i(0)  -  £2(0)  direction. 

cos(los)  =  1_V2  •  Irn  (17) 

and 

r  1 V2  X  ( 1 V2  X  ^  (0)  -  £2(0)))  -  1  v2  X  (0)  -  £2(0)) 

R  ~  1.V2’  II  >>  "  II  *  II  "  "  II 


If  we  also  uniformly  scale  distance  by  ri2  and  then  scale  time  by  V2,  we  are  left  with  a  4- 


dimensional  data  set: 


^3x3 

0lx3 


^3x3  ^3x1 

(l/ri2)R'^  -(l/ri2)R\2(0) 


0 


1x3 


1 


r 

1 

(Vi/V2)lvi 

0 

cos(los) 

0 

sin(los) 

for  i=l  and 

0 

0 

0 

1 

0 

L  J 

1 

dataj 


(19) 


for  i=2 


If  we  consider  some  additional  condition  on  the  shape  of  the  intercept  surface  (such  as  when 
two  siuface  components  are  disjoint)  we  can  express  this  condition  using  the  inequality: 

ViA^2  -  H(l^vi>  los)  for  some  function  H  (20) 

In  the  next  section,  we  obtain  a  simple  expression  for  when  the  surface  components  are  dis¬ 
joint,  by  finding  another  function,  HGos),  which  satisfies  the  inequality: 

H(lvi,  los)  <  H(los)  for  all  lyi  (21) 

We  can  then  ensure  disjoint  surface  components  whenever: 

V1W2  ^  HOos)  (22) 

To  show  the  tightness  of  the  bound,  we  also  deteimine  the  value  of  l^vi»  makes  the  above 
inequality  become  an  equality. 

3.3  Components  of  the  Intercept  Surface 

The  two  points  £1(0)  and  £2(0)  are  both  on  the  intercept  surface. 

The  scalar  function  F(r),  whose  zero  set  is  the  intercept  surface,  is  continuous  everywhere 
except  at  the  the  two  points  £i(0)  and  £2(0). 
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We  will  determine  conditions  on  the  data  that  ensure  that  the  solution  component  that  includes 
the  point  £1(0)  is  disjoint  from  the  solution  component  that  contains  the  point  £2(0). 

Figures  2,  3,  and  4  show  cases  where  the  components  are  separate,  touching,  and  merged. 
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v2/v1  =3.129 


3 


Figure  2:  Intercept  Surface  With  Two  Separated  Components 

x2-x1  =(0.1 0  3.00  -2.00  ),  VI  =(1 .1 44  0.1 00  0.1 00  ),  V2=(0.1 00  2.000  3.000  ) 


v2/v1  =3.121 


Figure  3:  Intercept  Surface  With  Two  Touching  Components 

x2-x1  ={0.1 0  3.00  -2.00  ),  V1  =(1 .1 47  0.1 00  0.1 00  ),  V2=(0.1 00  2.000  3.000  ) 


haIfcone(eta1 )  haIfcone(eta2)  eHipsoid(eta1  ,eta2)  0<eta1<pi  0<eta2<pi 


v2/v1  =3.113 


Figure  4:  Intercept  Surface  With  Merged  Components 
x2-x1=(0.10  3.00  -2.00  ),  V1=(1.150  0.100  0.100  ),  V2=(0.100  2.000  3.000  ) 


The  point  £1(0)  has  Tj  =  0,  so  for  all  0  <  112^  <  it,  it  is  contained  in  the  solid  ellipsoid: 


.  IIXi(Q)ll  ^2^ 

■  Iiy2(0)||  sin(Tl2_,)  ’ 

For  all  0  ^  Ti2^  ^  n,  the  point  £2(0)  is  at  the  vertex  of,  and  therefore  is  contained  in,  the 
solid  half-cone: 

TI2  ^  Tl2_  (24) 

Theorem  1:  For  any  fixed  0  <  112^  ^  tu,  the  intercept  surface  is  contained  in  the  union  of 
the  solid  ellipsoid  and  the  solid  cone  described  above. 

To  ensure  that  the  solid  half-cone  TI2  ^  Ti2^  does  not  intersect  the  solid  ellipsoid,  we  must 

restrict  the  line-of-sight  Gos)  angle  between  the  cone  axis,  y2(0),  and  the  ellipsoid  axis, 

£2(0)  -£i(0). 


Define  the  line-of-sight  angle: 


los  =  cos' 


'  Y2(0)  t 

J1V2(0)1| 


(11(0) -£2(0)3^ 
llri(O)  -£2(0)11^ 


Theorem  2:  If  0  ^  vl/v2  and  0  <  los  <  7t  satisfy: 


l|Vi(0)H  ^  max 
||V2(0)1|  0<los<n2_<Ji 


«n(^0 


sin(Ti2^  -  los) 


sin(Tl2_)  1 


^1* 

1 

1 

tan(Tl2_)_ 

(25) 


(26) 
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where 


=  (tc  -  sin  ^(vl/v2)  +  los)/2 


(27) 


then  the  following  equation  has  a  solution  with  0  <  los  <  ^2^  <  ^ 


l|Vi(0)ll  sin(Tl2_) 

Sin(Tl2^  -  los) 


IIY2(0)I1 


(28) 


For  for  the  largest  such  ^2^  solution,  the  solid  ellipsoid  and  solid  half-cone  kiss.  For  values 
of  '>12^  just  smaller  than  that  solution,  the  solid  ellipsoid  and  solid  half-cone  are  disjoint,  so 
the  solution  components  that  contain  the  points  rj(0)  and  ^(0)  are  disjoint. 


Conjecture:  When  the  two  solution  components  just  touch,  the  max  intercept  time  on  the 

i 

component  that  encloses  the  slower  vehicle,  occurs  at  the  touch  point. 


Proof  of  Theorem  1:  On  the  intercept  surface,  F(r)  =  0,  yet  in  the  region  of  space  that  is 
exterior  to  both  the  half-cone  and  the  ellipsoid  we  have: 


Fir)  =  rj  -  k  r2 


||Vi(0)l|  Tl2  sin(Tii) 
|IV2(0)1I  sin(Ti2)  Hi 


l|Vi(0)H  H2 

||V2(0)I|  sin(Ti2) 


for  all  0  <  Til  <  7U 


IIYi(0)ll  ^2^ 

||V2(0)||  sin(ti2_) 


for  0  <  Ti2  <  ^ 


(29) 

i.e.  outside  the  solid  half-cone 


>  0  outside  the  solid  ellipsoid 

So  the  region  of  space  that  is  exterior  to  both  the  solid  half-cone  and  the  solid  ellipsoid  has 
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Tj  >  T2  so  cannot  contain  points  on  the  intercept  surface  (where  Tj  =  T2). 


For  any  point  along  the  ray  Tii  =  0  that  is  both  inside  the  solid  ellipsoid  and  inside  the  solid 
half-cone,  the  inequalities  in  the  above  proof  are  reversed  and  <  T2. 

Proof  of  Theorem  2:  As  T|2^  varies,  the  point  where  the  solid  half-cone  and  the  solid  ellip¬ 
soid  first  touch  is  contained  in  the  plane  that  contains  the  cone  axis,  V2(0)  and  the  ellipsoid 
axis,  £2(0)  -rj(0).  In  this  plane,  the  cone  becomes  two  rays  based  at  £2(0).  The  angles 
between  these  rays  and  the  cone  axis  are  I  be  ^  vector  along  the  ray  with  angle 

-l-T|2^.  This  ray  can  intersect  the  planar  ellipse  at  up  to  two  points.  When  the  two  solutions 
coalesce,  the  cone  and  ellipse  are  tangent  to  each  other. 


The  law  of  cosines  applied  to  the  triangle  with  vertices  £i(0),  £2(0),  and  r  can  be  written  as: 

=  r2^  +  r|2^  -  2  r2  T12  cos(t|2  -  los)  (30) 


The  boundary  of  the  solid  ellipsoid  is  given  by: 


IIVi(0)ll  ^2^ 

Iiy2(0)||  Sin(Tl2„) 


Eliminating  rj  in  the  above  two  equations  gives: 


0  = 


11X2(0)11  sin(Tl2_) 


^2^  +  ri2^  -  2  r2  ri2  cos(q2^  -  los) 


(31) 


(32) 


This  quadratic  equation  has  two  solutions  that  give  the  normalized  distance  r2  along  the  cone 
ray. 

This  quadratic  equation  has  roots: 
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Jl 

ri2 


(33) 


C0S(T12^  —  los)  ± 


'\j COS^(Tl2^  -  los)  -  (1  - 


l|Vl(0)ll  ^2^  2. 

||V2(0)||  Sin(n2_)  ^  ^ 


(l-[ 


l|Vl(0)ll  ^2^  2' 

|IV2(0)11  sin(Ti2_)  ■ 


The  two  roots  coalesce  when 


I  HVi(0)ll  ^2^ 

V  ^  ^  ||V2(0)1|  sin(Ti2_) 


or 


(34) 


>  or 


lsin(Ti2^  -  los)  I  = 


l|Vi(0)ll  ^2^ 

11X2(0)11  sin(Ti2_) 


solve  for  n-? 


(35) 


l|Vi(0)|| 

11X2(0)11 


lsin(Ti2^-  los)l 


(36) 


To  obtain  the  largest  value  of 


IIYi(0)|l 

11X2(0)11 


for  which  solutions  to  equation  36  exist,  note  that 


(.99  -  sin(^))*.724  < 


max 

0<los<r|2^<jt 


^2^ 


S“(^2^  -  los) 


<  (1.03  -  sin(los/2))*.724 


and  the  maximum  occurs  at  the  value  of  Ti2^  given  by: 

sin(2T|2_^  -  los)  =  vl/v2  2  branches  for  sin“^  (38) 

We  need  Ti2^  in  the  range  0  <  los  <  112^  <  7t  so  we  choose  the  branch  with  the  largest 
value  of  Ti?  _ ,  e.g. 

Ti2^  =  (7t-asin(vl/v2)+los)/2  (39) 
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The  above  bounds  imply  that: 


equation  36  has  a  solution 

if 

llXi(0)ll 

11X2(0)11 

<  .724*(.99  -  sin(los/2)) 

(40) 

equation  36  has  no  solution 

if 

o  o 

^  .724*(1.03  -  sinaos/2)) 

(41) 

After  solving  equation  36  for  112^,  the  location  of  the  point  where  the  solid  ellipsoid  and  the 
solid  half-cone  just  touch  can  be  determined  by: 


lL=  —1— 

ri2  cos(il2^  - 


Ji_ 

^12 


=  tan(Tl2_  - 


los) 

(42) 

los) 

(43) 

Examples: 


For  fixed  values  of  los,  we  can  plot  equation  36  to  find  the  value  of  Ti2^  >  los  that  gives  the 
l|Vi(0)|| 


largest  value  of 


11X2(0)11 


los  =  1.57  rad  =  90  deg 


los  =  1.00  rad  =  57  deg 


gives 


min  11X2(0)11 
Tl2_  [llXi(0)ll 


4.60 


min  11X2(0)11 
|lXi(0)ll 


2.56 


at  Tl2_  =  2.25  (44) 
at  tl2_=1.87  (45) 
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Tight  Bound 

The  inequality  in  theorem  2  becomes  an  equality  when 


ViA^2  = 


max 

0<T|2^<los<7t 


sin(Tl2^ 


sin(Tl2c^  -  los) 


.742  (1  -  sin(los/2))  (46) 


which  occurs  at 


n? _ ^  =  (tc  -  sin  ^(vl/v2)  +  los)/2 


(47) 


The  inequalities  in  theorem  1  become  equalities  when  T|i  =  0  and  TI2  =  'n2.^  (solution  of 
equation  in  theorem  2).  This  implies  that  the  triangle  with  sides  of  length  r^,  T2,  and  ri2  is  a 
right  triangle,  since: 

Vi(0)  t\2^  r, 

stadia -los)=—^r;^  =  k  =  -  when  m  =  0,  = 

To  orient  the  Vi(0)  vector  in  the  direction  that  results  in  qj  =  0  at  the  point  where  the  cone 
and  ellipsoid  meet,  we  need: 

1)  Vj(0)  must  be  in  the  plane  spanned  by  the  cone  axis,  V2(0),  and  the  ellipsoid  axis, 
12(0)  -  ri(0). 


2)  Vi(0)  must  be  perpendicular  to  the  ellipsoid  axis,  £2(0)  -  rj(0). 

This  can  be  done  by  pointing  Vi(0)  along  the  direction: 

(r2(0)-ri(0))  X  [(12(0)- £1(0))  x  V2(0)]  (49) 
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3.4  Singularities  on  the  Intercept  Surface 


The  two  points  ij(0)  and  £2(0)  are  both  on  the  intercept  surface. 

The  function  F(r)  that  defines  the  intercept  surface  is  continuous  everywhere  except  at  the  the 
two  points  £1(0)  and  £2(0). 

To  determine  if  the  intercept  surface  has  any  other  singularities,  we  can  examine  the  gradient 
of  the  function  F(^  to  see  if  it  is  bounded  away  ftom  zero.  Figure  5  shows  a  case  where  the 
intercept  surface  has  a  singularity. 

The  following  notation  is  useful  in  deriving  the  gradient  of  F: 


r-£i(0) 

Ti 


i  =  1,2  unit  vectors 


(50) 


(£-£i(0))  X  fr-£i(0))  X  WO) 

h.  =  ^  -*  — ^  -IV  -1^  2  (51) 

-*  ||Q:-£i(0))x(£-£i(0))xVi(0)|| 

Note  that  for  each  i,  Ij.  and  1^  are  unit  vectors  and  are  orthogonal  to  each  other,  so  we  can 
define  the  following  rotation  matrices: 

Ci  =  [l,,hi,l„xhi)  i=l,2  (52) 
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Figure  5:  ^1/^2  that  satisfies  separate  solution  component  bound 


( (2/soi)u|S  -  eo*  I )  "vZL  >  SA/I.A  >  ( {3/S0|)u!S  -  66' ) 


The  normal  to  the  surface  is  given  by  the  gradient  of  the  function  that  defines  the  surface. 


dF  _  ^  ^  ^  dk  ^ 

dr  ^  ^ 


(53) 


To  evaluate  recall  that 
dr 


k= 


|lVi(0)||  sin(tii)  Tl2 


11X2(0)11  Til  sin(Ti2) 


(54) 


d  sin(Tii)  ^  _  sin(Tli)  i  _  1 

dr\i  Til  ”  “ni  .Si  tan(Tii) 


) 


(55) 


— )  =  (—Zl? — )  ( J - 1 — 

dTl2  sin(Tl2)  sin(Ti2)  Tl2  tan(Ti2) 


) 


(56) 


Combining  the  above  3  equations  gives: 

||Vi(0)i|  sin(Tii)  Tl2 


dr 


11X2(0)11  Til  sin(Tl2) 


1 


dTii 


tan(Tii) 


(-f- 

^2 


1  _^dTl2 


tan(T|2)  ^ 


=  -k 


1  1  dTii  _  1  1  dTi2 

Til  tan(T|i)'^  dr  ^T|2  tan(T|2)'^ 


(57) 


dTli 

The  angle  from  the  vector  Xi(0)  and  r  -  ij(0)  is  Tii,  so  is  a  vector  of  length  l/q,  perpen¬ 
dicular  to  r -rj(0),  in  the  plane  spanned  by  Xi((J)  and  I-Ii(O).  The  unit  vector  1^,  defined 
above,  is  in  the  appropriate  direction,  so 


dTli  1  . 

-  =  -  h: 

dr  Ti 


(58) 
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Combining  the  above  equations  [and  noting  that  —  =  k  when  F(r)  =  0  ]  gives: 

^2 


dr 


1 


Til  tan(Tii) 


)  hi 


li,  +  ( 


1 


1 


Ti2  tan(Ti2) 


)  h 


ii2 


1 

1 

1  1 

-kC2 

1  1 

Til  tan(Tii) 

Ti2  tan(Ti2) 

0 

0 

Applying  the  triangle  inequality  to  the  above  equation,  we  get: 

ll^ll  ^  +  - -  Ikl  -Jl  +  C-i - 

(k  \  tan(Tii)  \  Tl2  tan(q2) 


f  IIYi(0)ll  Ti2 

[l|V2(0)||  sm(r\i) 


tan(Ti2) 


(59) 


(60) 


The  gradient  cannot  be  zero  in  regions  of  space  where  TI2  satisfies: 


11X2(0)11 


^2 


'V 


^  1  1 

^^2  tan(q2) 


(61) 


||Vi(0)||  sin(Ti2) 

This  represents  a  half-cone  region  in  front  of  the  missile,  up  to  an  angle  of  TI2  from  the  velo¬ 
city  vector  V2(0).  This  means  that  the  intercept  surface  cannot  have  any  singularities  in  that 
region  of  space,  except  at  the  point  r  =  tjfO)  where  the  function  itself  is  discontinuous.  The 
function  is  also  discontinuous  at  the  point  r  =  12(0),  but  that  point  is  outside  the  TI2  bound 
given  above. 


^  11X2(0)11  ^ 

For  example,  =  3 

T|2  <  1.97rad  =  112deg. 


then  the  gradient  cannot  be  zero  in  regions  where 
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All  possible  singular  points: 


In  an  earlier  section,  we  showed  that  the  12-dimensional  set  of  data:  ri(0),  £2(0);  Vi(0),  V?(0) 
could  be  reduced  to  a  4-dimensional  set  by  modding  out  by  the  6-dimensional  Euclidean 
group  of  motions,  scaling  distance,  and  scaling  time.  For  each  point  in  the  remaining  4- 
dimensional  data  set,  we  get  a  2-dimensional  solution  set  for  r  e  R^.  The  vector  equation, 
dF/ck  =  0,  is  equivalent  to  3  scalar  equations,  which  give  fixed  points  on  the  2-dimensional 
solution  set,  and  put  a  single  condition  on  the  4-dimensional  data  set.  Therefore,  there  is  a 
3-dimensional  set  of  degenerate  data,  that  results  in  the  solution  surface  having  a  singular 
point.  This  3-dimensional  set  of  degenerate  data  can  be  parameterized  by  the  following  rota¬ 
tion  matrix: 

C21  C12  Ci3 

C  =  C21  C22  C23  —  C2  €  SO(3)  (62) 

C31  C32  C33 


The  solution  set  is  singular  when 


i.e., 

=  k  (Ci^  C2) 

We  can  solve  these  three  equations  for  k,  - -  and  - r— 

^  Til  tan(Tli)  Tl2  tan(Ti2) 

_1 _ 1_  _  ~^31 

Til  tan(Tii)  C32 


1 

1  1 
Til  tan(Tii) 

0 


1 

1  1 
TI2  tan(Ti2) 

0 


(63) 


(64) 


(65) 
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J_  _  1 

712  tan(Ti2) 


(66) 


Cl3 


k  = 


tan(Tii) 


tan(Ti2) 


1  + 

^32 

1  +  (— )^ 
C23 


(67) 


The  above  three  equations  can  then  be  used  to  solve  for  Tii,  TI2.  and  ||Vi(0)I|/||V2(0)||  as  func¬ 
tions  of  C.  These  values  of  Tji  and  TI2,  give  a  point  on  the  2-diniensional  solution  set.  The 
4-dimensional  data  set  was  parameterized  by  llVi(0)|l/l|V2(0)|l,  los-angle,  and  l^Vj  ^ 

3.5  Acceleration  Constraints  for  the  Pursuer 

In  this  subsection,  we  will  look  at  two  things.  First  we  will  derive  a  bound  of  the  form: 

vl/v2  <  functionGos,accel_limit)  (68) 

that  guarantees  that  the  pursuer  will  not  violate  its  acceleration  constraint  anywhere  on  the 
selected  component  of  the  intercept  surface.  Second,  we  will  derive  a  set  of  nonlinear  equa¬ 
tions  whose  solution  gives  initial  conditions  and  a  point  on  the  intercept  surface  where  the 
pursuer’s  acceleration  constraint  is  just  reached.  Figures  6-9  show  acceleration  constraints  on 
the  intercept  surface. 
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Figure  6:  Acceleration  Limits  On  Pursuing  Missile 
max  v1/v2  to  guarantee:  (Solid)  Separate  sol  components,  (Dotted)  accel2  <  [0.79, 1,00, 1.39] 


lTime=0.0  x2mi-xH01=n  2.Q -1 0.0  8.01  VH0t=f-0.3  0.3  -0.21  V2mi=f-0.7  0.5  -0.41  v2M  ^2.15  >?  1.39  error=5.1  e-005 


To  determine  the  bound  vl/v2  <  function(los,accel_liniit),  we  start  with  the  equation  that 
defines  the  intercept  surface: 


sin(Tli)  sin(Ti2) 

vl/v2  =  (rl/r2)- 


Tli 


T]2 


(69) 


and  the  equation  that  defines  the  surface  of  constant  acceleration  for  the  pursuer,  vehicle  2: 


l|V2(0)ll  = 


21|V2(0)|P  sin(Ti2) 


(70) 


In  equation  69,  we  can  rewrite  rl  as: 

ri  =  |I(r-ri(0))i| 

=  ll(ri(0)-r2(0))-(l-l2(0))II 
=  ll(ri(0)  - 12(0))  -  [1,2,  lv2^]  r2 
=  Il[iv2.  iv2-^f  (£i(0)  -12(0))  -  r2 


cos(los) 

COS(Tl2) 

lll'n 

sin(los) 

-^2 

sin(Ti2)cos(C2)  II 

.  0  . 

sin(Ti2)sin(C2)_ 

Putting  this  expression  for  rj  into  equation  69  gives: 


sin(Tii) 


cos(los) 

COS(Tl2) 

vl/v2  =  ||(ri2/r2) 

sin(los) 

sin(T|2)cos(C2) 

.  0  . 

sin(Ti2)sin(C2) 

sin(T|2) 

112 


(71) 


(72) 


Solving  equation  70  for  12  and  putting  that  into  equation  72  gives: 


sin(Ti2) 

■Hi 


vl/v2  = 


IIY2(0)I| 


IIY2,  r^(0),  ni=n/2ll  L 


cos(los) 

COS(T|2) 

sin(los) 

-  sin(Ti2) 

sin(Ti2)cos(C2) 

.  0  . 

sin(Ti2)sin(C2) 

J[_ 

II2 


(73) 


where 
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11^2,  r=Tj(0),  Ti2=it/2ll 


2|1V2(0)1P 


The  right-hand  side  of  equation  73  is  minimized  when  C2  =  0-  If  we  consider  examples 
where  the  Vi(0)  vector  is  rotated  till  Tii  =  0  at  the  point  where  the  intercept  surface  and  the 
acceleration  constraint  surface  just  touch,  we  get: 

/  ^ min  „  [cosOos)]  ^  cos(t12)  „  1 

=  [sin{tl2)J".^  . 


Actually,  the  min  should  be  taken  over  the  restricted  interval: 

min 

0<'n2<(3  los  -I-  Jc)/4 

to  ensure  that  we  are  finding  the  intersection  of  the  constant  accel2  surface  with  the  com¬ 
ponent  of  the  intercept  surface  that  contains  r^CO). 


Equation  75  gives  the  value  of  vl/v2  that  corresponds  to  the  intercept  surface  just  touching 
the  accel  constraint  surface.  Larger  values  of  vl/v2  could  result  in  either  larger  or  smaller 
values  of  max  acceleration.  To  eliminate  cases  where  smaller  values  of  vl/v2  result  in  larger 
accelerations,  we  can  set 


0  whenever  sin(q2^^)  > 


2||V2(0)|P 


(v  1 


(vl/v2)jo^h  whenever  sin(q2,^) 


2||V2(0)|P 
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Examples  where  acceleration  constraint  is  just  met 


I 


Constraining  a  vehicle  to  a  fixed  acceleration  magnitude,  means  that  it  will  be  on  a  fixed 
radius  circular  path  (tangent  to  its  velocity  vector).  The  set  of  all  such  circular  paths  forms  a 
2-dimensional  surface.  The  fixed  value  of  acceleration  that  just  causes  this  surface  to  touch 
the  intercept  surface  is  the  max  acceleration  on  the  intercept  surface.  To  solve  for  the  inter¬ 
section  point  of  these  two  tangential  surfaces,  we  must  set  the  gradient  of  the  constant 
acceleration  surface,  equal  to  a  constant  times  the  gradient  of  the  intercept  siuface.  The  mag¬ 
nitude  of  the  acceleration  on  the  two  surfaces  must  also  be  set  equal.  This  gives  3  equations 
to  solve  for  the  magnitude  of  the  acceleration  and  the  point  on  the  two-dimensional  intercept 
surface  where  that  acceleration  occurs. 


The  acceleration  of  vehicle  i  is  given  by: 

accelj  =  [L.,  Iv] 


sin(Tii) 

cos(Tii) 


2||Vi(0)lP  sin(Tii) 


(77) 


so  the  magnitude  of  the  acceleration  is  constant  on  the  2-dim  surface  defined  by  the  function: 

211Vi(0)lP  sin(Tii) 


Hacceljll  = 


For  fixed  values  of  ||acceli||,  the  gradient  of  this  function  is: 


(78) 


dr 


21|Vi(0)|p  sindii) 


Ti 


=  [lx.  hi] 


-sin(Tii) 

COS(Tli) 


2||Vi(0)|p 


=  Ci 


-sin(T|i) 

cos(Tii) 

0 


2||Vi(0)|p 


09) 


dllaccel-jll  dF  _  .  ,  ,  . 

Setting  a  scalar  times  -  equal  to  —  gives  three  equations  for  that  scalar  and  the 

®  dr 

touch  point  on  the  2-dimensional  intercept  surface: 
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-sm(Ti2) 
C2  cosCiij) 
0 


tan(Tii) 

0 


tanCTij) 

0 


tan(Tli) 


=  (Ci’^cy 


tan(Ti2) 


-sin(Ti2) 

COS(T|2) 


Solving  for 


tan(Tli) 


,  k,  and  a,,  gives: 


1  1 
Til  tan(Tii) 


j^l  TI2  cos(Ti2)  sin(Ti2)  Cii  C21  r  j 

^  sin(Tl2)  _r_l _ L_]  1  ^2  _ 1 _ 

112  tan(Ti2)  Til  tan(T|i) 


ni 

sin(Ti2) 


_  £11 

COS(Tl2)  Sin(7l2)  ^11  ^21 

.r^ _ L_1  1  ^13 

‘t)2  tanCna)^ 


Since 


I|Vi(0)l|  sin(Tii)  TI2 
||V2(0)||  Til  sin(Ti2) 


we  can  combine  equation  85  with  the  top  half  of  equation  83  (for  k)  to  get: 
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l|Vi(0)ll 

mm 


sin(T]i) 

Til 


=  [C0S(Tl2),  sin(Ti2)] 


Cll  “  C21 


C12  -  C22 


^13 

C23 

Cl3 

C23 


(86) 


In  the  next  two  sections,  we  will  present  both  iterative  and  closed-form  techniques  for  calcu- 
lating  points  on  the  two-dimensional  surface  defined  by  the  scalar  equation  F(r)  =  0. 
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4.  Iterative  Algorithm  for  Computing  the  Intercept  Surface 


This  section  defines  an  iterative  algorithm  for  computing  the  radius,  rj,  along  each  fixed  unit 
vector,  such  that  the  vector  r  =  ijfO)  +  rj  is  on  the  intercept  surface. 

From  equations  8  and  9, 

ri  =  |t-ri(0)||  r2  =  It- 12(0)11  (87) 

Using  the  law  of  cosines  on  the  triangle  with  vertices  ij(0),  £2(0)  and  r,  we  can  write  r2  as  a 
function  of  Tj,  Hi, 

=  ri^  -  2  cos((t))  ri  Hrj(O)  -  £2(0)11  +  Iti(O)  -  r2(0)lp  (88) 

where  <})  is  the  angle  between  the  vectors  ri(0)  -£2(0)  and  r  -  ri(0).  This  angle  is  fixed  dur¬ 
ing  the  iteration,  since  the  unit  vector  along  r- £2 (0)  is  only  a  function  of  Hi  and  Ci- 


From  equation  8,  we  also  see  that  H2  is  the  angle  between  V2(0)  and  r -^(0),  so  H2  can  be 
written  as  a  function  of  ri.  Hi.  Ci- 


1  Y2(Q)  >  (£-12(0)) 

IIY2(0)I1  1^-12(0)11 


(89) 


where 


r-r2(0)=[ri(0)-r2(0)]  +  ri 


Vi(0) 

llVi(0)II’ 


Vi(0)^ 


cos(Hi) 

sin(Hi)cos(Ci) 

sin(Hi)sin(Ci) 


(90) 


Given  fixed  values  for  (Hi.Ci).  and  an  initial  value  for  ri,  we  can  use  equations  87-90  to 
evaluate  r2  and  H2.  use  equation  12  to  evaluate  the  new  value  of  ri.  This  gives  an  itera¬ 
tion  for  ri. 
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If  we  use  Til  Cl  ccxirdinates  on  the  two-dimensional  intercept  surface,  we  can  use  the 
iteration  defined  above  to  solve  for  the  corresponding  values  of  ri.  Then  equation  8  gives  the 
Cartesian  coordinates  of  the  point. 


When  Til  =  7t,  equation  12  is  satisfied  by  ri  =  0.  This  point  is  on  the  intercept  surface 
because  the  evading  aircraft  could  fly  on  any  circle  just  large  enough  so  that  the  aircraft 
returns  to  its  starting  point  in  the  time  it  takes  the  missile  to  get  to  the  aircraft’s  starting  point 


In  the  iterative  procedure,  we  will  make  use  of  the  following  formula  for  TI2  in  terms  of  los 
and  T 


W2i0f 


V2(0)'’ 


lri(0)-r2(0)]  + 


IIY2(0)1| 

=  ri2  cos(los)  +  ri  cosfy) 


Y2(0r 

l|V2(0)I| 


[r-ri(O)] 


(91) 


To  determine  a  two-dimensional  array  of  points  ri(Tii.,^i.)  on  the  intercept  surface  and  evalu¬ 
ate  the  cost  functional  at  each  point,  we  can  use  the  following  procedure  (given  in  more  detail 
later): 
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kO  =  (vlA^2)*sin(etal)/etal 

cos_los  =  -  x2_ininus_xl_unit  dot  V2_unit 

Set  TiCtIi  =  0  = 

For  j  =  0  to  30 

Til.  =  ((30-j)/30)(7t)  +  e  e>0  to  avoid  (sin(0))/0 
sel  =  sin(etal)  cel  =  cos(etal) 

For  i  =  -30  to  30 
Ci,=  (i^30):c 

szl  =  sin(zetal)  czl  =  cos(zetal) 

x_minus_xl_unit  =  [Vl_unit,  Vl_perp]*[cel,  sel*czl,  sel*szl] 

cos_phi  =  x2_niinus_xl_unit  dot  x_niinus_xl_unit 

cos_gam  =  V2_unit  dot  x_minus_xl_unit 

Initialize  rj  =  2ri(Tij^j,^i.)  -  (linear  extrapolation) 

Do  four  iterations  of;  (equations  12  and  87-91) 
rl_over_r2_old  =  rl_over_r2 

r2_over_rl2  =  sqrt(rl_over_rl2^  -2*cos_phi*rl_over_rl2  +  1) 
eta2  =  acos((cos_los+rl_over_rl2*cos_gam)/r2_over_rl2) 
k_of_eta  =  kO  *  eta2  /  sin(eta2) 
rl_over_rl2  =  k_of_eta  *  r2_over_rl2 
rl(ij)  =  rl_over_rl2*rl2 
r2(i  j)  =  r2_over_rl2*rl2 

T1  =  (rl/vl)  *  etal  /  sin(etal)  save  (ij)  if  max  time 

T2  =  (r2/v2)  *  eta2  /  sin(eta2)  T1  =  T2  if  converged 

accell(ij)  =  2  *  etal  *  vl  /  T1  (angle*radius=v*T  accel=v*v/r) 

accel2(i j)  =  2  *  eta2  *  v2  /  T2  (angle*radius=v*T  accel=v*v/r) 

End  i  loop 
End  j  loop 
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accelj  =  y 


sin(Tii) 

COS(T]i) 


2T1iliVi(0)li 

Tj 


i=  1,2 


(92) 


so 


llaccel;!!  = 


21|Vi(0)||2  sin(Tii) 


i=l,2 


(93) 


If  we  assume  10  floating-point  operations  each  for  computing  atan2,  sin,  and  sqrt,  then 
evaluation  of  the  inner  loop  equations  takes  approximately  31  floating-point  operations. 
Doing  4  iterations  on  a  30  x  60  grid  requires  a  31*4*30*60  =  223,000  floating-point  opera¬ 
tions.  The  outer  loops  of  the  iteration  account  for  an  additional  27,000  floating-point  opera¬ 
tions.  This  gives  a  total  of  approximately  250,000  floating-point  operations.  A  1  Mflop  com¬ 
puter  could  update  this  guidance  algorithm  at  4  Hz. 


Convergence  of  Iteration 


Theorem  3:  The  iteration  converges  at  points  on  the  solution  surface  where: 

(r-ri(0))'^-^^<0  (94) 

i.e.,  whenever  the  surface  is  star-shaped. 


Proof: 


The  iteration  is  of  the  form: 

■•l..  =  G(rwW  (95) 

where  is  a  unit  vector  based  at  the  point  ij(0),  and  rj  is  the  distance  along  that  unit  vector. 
For  each  fixed  value  of  the  unit  vector,  the  iteration  has  a  fixed  point  at  the  value  of  rj  that 
gives  the  point  on  the  solution  surface: 
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(96) 


I  =  Ii(0)  +  ri  Ij, 


We  define  the  function  G  by: 


Then 


G  =  rj  -  F(r)  =  k  r2 


dG  ^  ^  dF 

dTj  drj 


1-1'^^  = 
dr 


1-  1  -  k  hj  1  ^  1 

Ti2  tanCn2) 

^  L 

=  ki,7[i„,hj  ^  ^  ^ 

r\2  tan(ri2) 


The  iteration  converges  when  I- — I  <  1,  which  is  ensured  when 


a/i  +  C-J-- 
\  ^2 


t\2  tan(Ti2) 


y<  1 


iiy2(0)ii  ^  ^2 

|lVi(0)lI  "  sin(Ti2) 


112  tan(Ti2) 


(100) 


Note  that  this  is  the  same  bound  found  for  ensuring  that  the  intercept  surface  had  no  singulari¬ 
ties  in  regions  of  space  (values  of  TI2)  that  satisfied  the  above  inequality  for  some  given 

IIY2(0)|| 

IIYi(0)I|  • 
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Finally, 


1,T^=1_^>0 

(k  drj 


whenever 


1^1 

dr. 


T  dF 

So  the  iteration  converges  whenever  ^  >  0 


dr 


Note  that  the  function  — - —  has  value  0,  and  slope  +1  at  TI2 

112  tan(7i2) 

cally  increasing,  going  to  infinity  at  TI2  =  it. 


The  function  k(Ti2)  is  monotonically  increasing  as  T|2  goes  from  0  to  7t. 
goes  to  -H»  as  Ti2  goes  to  Jt. 


<  1  (101) 


0,  and  is  monotoni- 


The  function  k(Ti2) 
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5.  Algebraic  Algorithm  for  Computing  the  Intercept  Surface 


For  cases  where  the  iteration  defined  in  the  previous  section  does  not  converge,  we  can  solve 
algebraically  for  all  points  on  the  two-dimensional  intercept  surface  in  R^.  We  can  use  Tii 
and  112  ^  coordinates  on  the  two-dimensional  surface.  For  fixed  Tii  and  TI2,  equation  12 
defines  an  ellipsoid  in  R^.  Equation  8  indicates  that  fixing  Tij  defines  a  circular  cone  in  R^ 
with  vertex  at  ri(0)  and  axis-of-symmetry  V^CO).  Equation  8  also  indicates  that  fixing  112 
defines  a  circular  cone  in  R^  with  vertex  at  ^(0)  and  axis-of-symmetry  V2(0).  The  intersec¬ 
tion  of  these  three  quadratic  surfaces  in  R^  results  in  8  (some  possibly  complex)  solution 
points  for  each  fixed  pair  (t|  1,112). 

To  get  three  second-order  algebraic  equations,  we  will  use  Cartesian  coordinates  for  the  ellip¬ 
soid  in  equation  12,  and  for  the  two  cone  equations:  Tii  =  constant,  112  =  constant.  Squaring 
equation  12  gives: 


llr-ri(0)|p  = 


l|Vi(0)ll  1)2  sindii) 

||V2(0)11  Til  sin(ll2) 


llr-l2(0)lP 


(102) 


If  we  let: 

l|Vi(0)l|  ii2  sin(rii) 

k  =  ."-ll.:!!— li.  ILL  (103) 

I1V2(0)1|  Til  sin(Ti2) 

then  equation  102  becomes: 

It-rj(0)|p  =  k2l|r-i2(0)ip  (104) 

which  is  a  second-order  algebraic  equation  for  a  circular  ellipsoid  in  R  . 

Setting  Til  =  constant  gives  a  circular  half-cone  with  vertex  at  rjfO)  and  axis-of-symmetry  Vj(0) 
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[l-Ii(O)]^  Yi(0)  =  cos(Tii)  I|Vi(0)I|  It  -  ri(0)I|  i=1.2 

Squaring  equation  105  gives  a  second-order  polynomial  equation: 

0  =  [r  - [vi(0)Vi(0f  -  I  cos^dii)  IIVi(0)|p]  [r  - ^(0)]  i=l,2 


(105) 


(106) 


Equation  106  is  for  a  full  cone,  instead  of  a  half-cone,  since  the  sign  of  cos(t|j)  is  ignored. 


Together,  equations  104  and  106  are  three  second-order  equations  in  the  three  Cartesian  com¬ 
ponents  of  r.  By  Bezout’s  theorem,  we  expect  up  to  eight  solutions  for  each  fixed  set  of 
(^it'Hz)- 


To  solve  the  three  equations,  we  first  rotate,  translate,  and  uniformly  stretch  the  coordinates  to 
move  the  two  cone  vertices  to  x  =  ±1: 


1 

-1 

0 

12(0) 

0 

0 

.0. 

(107) 


We  then  rotate  the  coordinate  system  about  the  x  axis  (leaving  the  above  two  points  fixed)  till 
Vi(0)  has  no  z  component. 


In  this  new  coordinate  system,  let: 


X 

a 

K 

Yi(0) 

d' 

Y2(0) 

y 

A 

=  r 

D 

A 

■  COS(Tli)i|Vi(0)I| 

C 

X 

■  COS(tl2)||V2(0)I| 

(108) 


The  three  second-order  equations  can  then  be  written  as: 
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To  continue  the  solution  process,  we  introduce  a  bi-rational  transformation: 


(112) 


Since  this  only  differs  by  a  scalar  from  the  old  4-vector,  the  three  matrices  in  equations  109, 
110,  and  111  remain  unchanged: 
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0  = 


■ 

’l‘ 

T 

u 

V 

w. 

a^  -  1  0 


0 

a  b 
0 


a  b  0 

0  0  0 

0  b^  -  1  0 

0  0-1 


1 

'f 

u 

V 

w. 

conej 


(114) 


0  = 


" 

■f 

1 

T 

u 

V 

0  0  0  0 
Od^-1  de  df 
0  de  e^-1  ef 
0  d  f  e  f 


1 

“r 

u 

V 

-W. 

cone2 


(115) 


The  next  step  in  the  solution  is  to  get  a  combination  of  equations  113  and  114  to  be  of  the 
fonn:  u^  =  f(v)  and  =  g(v)  where  f(v)  and  g(v)  are  quadratic  polynomials  in  v.  Then 
equation  115  will  be  both  rearranged  and  squared  two  times  to  get  a  fourth-order  equation  in 
(u^,  v^,  w^).  By  replacing  u^  and  w^  with  f(v)  and  g(v),  we  then  get  an  eighth-order  polyno¬ 
mial  in  V  alone.  The  eight  v  solutions  can  be  used  to  obtain  the  eight  corresponding  values  of 
u  and  w. 

Taking  equation  113  plus  k^-1  times  equation  114  gives  an  equation  of  the  form  u^  =  f(v); 

k^  u^  =  1  -  (k^  -  1)  [(a^  -  1)  +  2  a  b  V  +  b^  v^]  (116) 


Equation  114  is  already  of  the  form  w^  =  g(v): 

w^  =  (a^  -  1)  +  2  a  b  V  +  (b^  -  1)  v^ 


(117) 


We  are  now  ready  to  begin  working  on  getting  equation  115  in  the  form  of  a  fourth-order 
equation  in  (u^,  v^,  w^).  We  begin  by  using  M  to  denote  the  3x3  nonzero  submatrix  in 
equation  115. 
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0=  V 


IT  mil  ^12  ^13 


mi 2  ni22  ^23 

mi3  m23  m33 


(118) 


Rearranging  equation  118  gives; 


T  “11  “12 


mio  m 


12  ^^^22 


+  w  m33  w  =  -2  w  [mi3,  m23] 


Note  that  when  the  [ri(0)£2(0).Yi(0),V2(0)]  data  is  planer,  we  get  f=0,  so  mi3  =  0  =  m23.  In 
that  case,  we  can  separate  the  uv  dependence  in  equation  119,  replace  u^  and  w^  with  qua¬ 
dratic  polynomials  in  v,  then  square  the  resulting  equation  to  get  a  quartic  equation  in  v. 


For  the  non-planar  case,  we  can  square  both  sides  of  equation  119  to  get: 


T  “ll  “l2 


“12  “22  LV. 


Expanding  and  rearranging  equation  120  gives: 


y]  +  “33  w4  =  4  w2  [^]  \  [mi3.  m23][[“ 


(120) 


“11  miim22+2mi2^  m33mii-2mi3^ 


“33“22~2m23^ 


Isymmetric 


(121) 


=  4  u  V  [-mi2mii  u^  -  mi2m22  v^  +  (2mi3m23  -  m33mi2)w^] 


Squaring  both  sides  of  equation  121  gives: 


.4F  r,.4i  r..4F 


2  2 
V  W 

0=  v^  A  v^  -h  4  v^  B  u^  w^ 

w'*  w'*  w'^  u^  v^ 


(122) 
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where  the  entries  of  A  are  given  by: 


aji  =  my'*  i  =  1,2,3 

a^j  =  +  8inij2(mi/  -  niiimjj) 


(123) 


and  the  entries  of  B  are  given  by: 

bji  =  my2(3mymaj  -  2m  -  4mii(mjjinii.2  +  my^nii/)  +  8miijmij(2miimjk  -  mucmij) 

(124) 

bij  =  inii2(miima^  -  201^^^)  (i,j4c)  cyclic  permutation  of  (1,2,3) 

If  we  substitute  equations  1 16  and  1 17  into  equation  122  for  each  occurrence  of  powers  of  u^ 
and  w^,  we  get  an  eighth-order  polynomial  in  v  alone.  We  can  put  the  coefficients  of  this 
polynomial  into  an  8  x  8  companion  matrix,  whose  eigenvalues  will  be  the  roots  of  the  poly¬ 
nomial. 

If  we  substitute  equations  116  and  117  into  equation  121  for  each  occurrence  of  u^  and  w^, 
we  get  a  linear  equation  in  u,  whose  coefficients  are  polynomials  in  v.  By  evaluating  these 
coefficients  with  the  8  values  of  v  found  above,  we  get  the  8  corresponding  values  of  u. 

If  we  substitute  equations  116  and  117  into  equation  119  for  each  occurrence  of  u^  and  w^, 
we  get  a  linear  equation  in  w  whose  coefficients  are  polynomials  in  u  and  v.  By  evaluating 
these  coefficients  with  the  8  values  of  u  and  v  found  above,  we  get  the  8  corresponding 
values  of  w. 

Finally,  we  can  use  the  right  part  of  equation  112  to  convert  [u,v,w]  back  to  [x,y,z].  This 
gives  us  the  [x,y,z]  values  on  the  two-dimensional  intercept  surface  for  each  value  of  the 
(Til,  T|2)  coordinates. 
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A  numerical  example  is  given  below. 


Input  Data: 

rl_  =[0.5163;  0.3190;  0.9866] 
r2_  =  [0.0606;  0.9047;  0.5045] 

Vl_  =  [0.2363;  0.0490;  -0.1546] 

V2_  =  [0.0782;  -0.3340;  0.3554] 
etal  =  1.5407 
eta2=  0.5354 

Computed  Results: 

k  =  0.5802 
los  =  0.4000 

noitn(Vl_)/norm(V2_)  =  0.5802 

eta2c  =  (pi  -  asin(norm(Vl_)/norm(V2_))  +  los)/2  =  1.4613 

(norm(V2_)/norm(Vl_))*(sin(eta2c)/eta2c)*sin(eta2c-los)  =  1.0236 
def  =  V2/(norm(V2)*cos(eta2))  =  [1.0709;  -0.4527;  0.0071] 
abO  =  Vl/(norm(Vl)*cos(etal))  =  [0.5895;  33.2048;  0  ] 

order8_poly  =  1936656295.  290948514.  -27327350.  -4381463.6  173893.7 
22468.886  -702,23409  -39.84748  1.34520 

uv0_coeff  =  55649.908255  3712.345422  -424.606279  -17.178161  1.15982968 

uvl_coeff  =  440.82806441 161 1  15.485303259795  -2.088513070591 

[  w0_coeff,  wl_coeff  ]  =  [  0.12675655356533,  0.07321379932120  ] 
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uvw  = 


8.3079 

-0.1243 

3.4003 

6.3804 

-0.0991 

-2.5165 

-7.3963 

0.0773 

-2.9874 

-5.8503 

0.0571 

2.2673 

-4.9642 

-0.0805 

-1.8345 

-4.4625 

-0.0738 

1.5786 

5.3506 

0.0505 

2.0264 

4.7569 

0.0426 

-1.7313 

%  Bi-Rational  transformation 
X(j)  =  (ua)+i)/(u(j)-i); 
ya)=  2*va)/(u0>l); 
z0)=  2*w0)/(u0>l); 


xyz  = 


1.2737 

-0.0340 

0.9306 

1.3717 

-0.0368 

-0.9354 

0.7618 

-0.0184 

0.7116 

0.7080 

-0.0167 

-0.6619 

0.6647 

0.0270 

0.6152 

0.6339 

0.0270 

-0.5780 

1.4597 

0.0232 

0.9315 

1.5323 

0.0227 

-0.9217 

These  eight  solutions  also  satisfy  the  HALF-cone  restrictions  of  equations  105. 
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The  computational  cost  of  evaluating  equations  103,  108,  118,  123,  and  124  is  approximately 
240  floating-point  operations.  The  cost  of  computing  the  eigenvalues  of  an  8  x  8  companion 
matrix  is  approximately  4*8^  =  2048  floating-point  operations.  The  cost  of  back-substitution 
into  equations  121,  119,  and  112  is  approximately  70  floating-point  operations.  This  gives  a 
total  of  approximately  2400  floating-point  operations  for  each  fixed  set  of  (rii,  'n2)-  we  use 
a  30  X  30  grid  for  (Hi,  Ti2),  then  the  total  computational  cost  is  approximately  2.1  million 
floating-point  operations.  This  is  approximately  eight  times  as  expensive  as  the  iterative  solu¬ 
tion. -A  2  Mflop  computer  could  update  the  algebraic  solution  of  the  guidance  algorithm  at 
approximately  1  Hz. 

Degeneracy  in  Back  Substitution: 

4 

When  equation  122  (the  eighth-order  polynomial  in  v)  is  the  square  of  a  fourth-order  polyno¬ 
mial,  equation  121  (made  linear  in  u)  gives  a  single  u  for  each  of  the  four  v  roots.  However, 
back-substitution  equation  119  (made  linear  in  w)  reduces  to  0  =  0,  in  which  case  we  can  use 
equation  117  which  gives  two  w  solutions  for  each  of  the  four  separate  (u,v)  pairs. 

When  equation  122  (the  eighth-order  polynomial  in  v)  is  the  fourth  power  of  a  second-order 
polynomial,  back- substitution  equations  119  (made  linear  in  w)  and  121  (made  linear  in  u) 
both  reduce  to  0  =  0,  in  which  case  we  can  use  equations  116  and  117  which  give  four  (u,w) 
solutions  for  each  of  the  two  separate  v  roots. 
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6.  Examples 


Figures  10  through  14  at  the  end  of  this  section  are  from  an  example  where  the  missile’s 
acceleration  constraint  is  not  exceeded  anywhere  on  the  intercept  surface  (except  near  the 
end-game).  Since  the  evading  aircraft  is  slower  than  the  pursuing  missile,  and  the  missile  is 
not  on  an  acceleration  constraint,  the  aircraft  is  completely  enclosed  by  the  intercept  surface 
and  cannot  escape.  Therefore  the  aircraft  heads  for  the  point  on  the  intercept  surface  that 
results  in  the  longest  intercept  time  (in  hopes  that  the  missile’s  fuel  runs  out).  The  missile 
assumes  that  the  aircraft  has  headed  for  the  point  with  the  longest  intercept  time,  so  the  mis¬ 
sile  also  heads  for  that  point 

The  guidance  algorithm  itself  could  update  at  80  Hz  on  the  120  MHz  Pentium  that  it  was  run¬ 
ning  on,  but  drawing  the  smoothly-lit  color  intercept  surface  on  the  screen  each  time  took  .5 
seconds,  so  in  this  example,  the  intercept  surface  was  only  recomputed  evety  .5  seconds. 
Each  .5  second,  both  vehicles  get  to  pick  new  values  for  their  optimal  constant  normal 
acceleration  inputs.  In  this  example,  it  takes  38  seconds  for  the  missile  and  aircraft  to  inter¬ 
cept 

Figures  15  though  18  are  from  the  second  example.  In  the  second  example,  the  same  initial 
positions  and  velocities  were  used  as  in  the  first  example,  but  the  aircraft  makes  no  attempt  to 
maneuver.  The  aircraft  flies  on  a  straight  path,  while  the  missile  still  assumes  that  it  will  head 
for  the  point  on  the  intercept  surface  corresponding  to  the  longest  intercept  time.  The  inter¬ 
cept  surface  shrinks  to  a  point  as  time  progresses,  causing  the  point  the  missile  is  headed  for 
and  the  point  the  aircraft  is  headed  for,  to  come  together.  Since  the  aircraft  has  not  headed 
for  the  point  that  would  result  in  the  longest  time  to  intercept,  intercept  occurs  sooner  than  in 
the  first  example  (29  seconds  instead  of  38  seconds). 

Both  of  the  above  examples  were  done  using  the  iterative  solution  for  the  intercept  surface. 
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Near  the  lower-right  comer  of  figures  10  through  18  is  printed  an  error,  e.g.  error  =  .0013  in 
figure  10.  This  is  the  amount  that  rj  changed  on  the  fourth  (final)  iteration.  The  vehicles 
start  out  around  8  "units  of  length"  apart  in  each  of  the  two  examples,  and  the  error  is  meas¬ 
ured  in  those  same  units. 
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Example  1(5  frames  from  a  simulation,  update  rate  =  .5  seconds) 


Aircraft  on  piece-wise  circular  path 
Missile  on  piece-wise  circular  path 
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Example  2  (  4  frames  from  a  simulation,  update  rate  =  .5  seconds) 

Aircraft  on  straight  line  path 
Missile  on  piece-wise  circular  path 
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7.  Summary 


The  proposed  mini-max  pursuit-evader  algorithm  allows  both  vehicles  to  maneuver  in  3D 
space  and  yet  is  fast  enough  to  run  real-time  on  today’s  flight  computers.  It  also  allows  for 
the  possibility  of  providing  the  pilot  of  either  a  missile-launching  aircraft,  or  a  missile-evading 
aircraft,  an  intuitively  clear  display  of  the  current  intercept  surface  with  constraints  and  cost- 
functional  clearly  displayed.  This  information  can  be  used  to  help  the  pilot  make  a  missile 
launch  decision. 

The  drawback  of  using  the  algorithm  on  the  missile  itself,  is  that  the  missile  would  need  velo¬ 
city  and  range  measurements,  either  from  its  own  sensors  or  from  the  launching  aircraft. 

We  intend  to  further  investigate  this  guidance  algorithm  and  its  behavior  for  various  types  of 
constraints,  cost  functionals,  and  initial  conditions.  We  also  would  like  to  compare  the  results 
to  more  conventional  mini-max  pursuit-evader  algorithms  that  do  not  make  the  piece-wise 
constant  speed,  piece-wise  constant  acceleration  assumptions  that  we  have  used. 

The  bounds  of  the  form:  vl/v2  =  H(los)  for  acceleration  limits  and  for  convergence  of  the 
iteration  should  be  extended  to  the  case  were  there  is  more  than  one  arc  in  the  piece-wise  cir¬ 
cular  trajectories. 
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Abstract:  A  new  technique  for  systematically  designing  nonlinear  regulators  is  introduced. 
The  method  consists  of  first  using  direct  parameterization  to  bring  the  nonlinear  plant  to  a 
linear  structure  having  state-dependent  coefficients  (SDC).  A  state-dependent  Riccati  equation 
(SDRE)  is  then  solved  at  each  point  x  along  the  trajectory  to  obtain  a  nonlinear  feedback  con¬ 
troller  of  the  form  u  =  — R“Hx)g(x)^P(x)x,  where  P(x)  is  the  solution  of  the  SDRE.  In  the 
case  of  scalar  x,  it  is  shown  that  the  SDRE  approach  yields  the  optimal  solution  of  the  non¬ 
linear  regulator.  In  the  multivariable  case,  it  is  shown  that  for  any  SDC  parameterization  that 
is  strongly  controllable  and  strongly  observable,  the  SDRE  method  produces  a  closed  loop 
solution  that  is  globally  asymptotically  stable  provided  that  the  state  and  control  weighting 
matrices  are  chosen  properly.  It  is  shown  that,  if  it  exists,  the  parameter-dependent  SDC 
parameterization  can  be  computed  such  that  the  multivariable  SDRE  closed  loop  solution  is 
optimal.  Additionally,  for  the  case  of  parameter  variations,  the  robustness  of  the  method  is 
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characterized.  A  general  nonlinear  minimum-energy  (nonlinear  problem  is  then  posed. 
For  this  problem,  the  SDRE  method  involves  the  solution  of  two  coupled  SD  Riccati  equa¬ 
tions  at  each  point  x  along  the  trajectory.  In  the  case  of  full  state  information,  it  is  shown 
that  the  SDRE  nonlinear  H„  controller,  assuming  properly  chosen  weighting,  is  internally 
stable.  Examples  are  provided  which  illustrate  the  effectiveness  of  the  SDRE  technique. 
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/*  missile. c  Draws  the  intercept  surface  of  an  aircraft  and  missile 

»  August  06,  1995  Mike  Elgersma 

*  November  05,  1995  Include  option  to  make  2  surface-components  tangent. 

»  November  19,  1995  Put  in  acceleration  constraints. 

*  November  19,  1995  Replaced  vector  iteration  with  faster  scalar  iteration. 

*  November  21,  1995  x_minus_xl_unit  -  [Vl_unit ,Vl_perp]* [cel,sel*szl,sel*czl] 

*  Algorithms  from;  Morton  &  Elgersma  ECALM  paper 

*  OpenGL  code  modified  from;  Feb.  1995  MS  Journal  pp  19-40 

- - */ 

#include  <stdio.h> 

#include  <stdlib.h> 

^include  <math.h> 

#include  <windows.h> 

^include  <GL/gl.h> 

^include  <GL/glu.h> 

#define  pi  3.1416f/«  slightly  bigger  than  pi,  so  surface  has  a  little  overlap.  •/ 

/*  Typically  choose  num_eta*30  and  num_zeta*60  since  zeta  has  twice  the  range, 

but  for  checking  the  point  where  two  surface  components  meet,  the  surface  gets 
stretched  in  the  eti  direction,  near  eta *^0,  so  use  more  eta  pointsv^^ 

#define  num_eta  30  /*  The  number  of  points  on  0  <  eta  <  pi  on  2-sphere  */ 

#define  num_zeta  60  /»  The  number  of  points  on  0  <  zeta  <  2*pi  on  2-sphere  •/ 

#define  num_iter  4  /»  number  of  iterations  to  find  radius  on  warped  2-sphere  */ 

void  initialize_state (float  xyzl_0 [3] ,f loat  xyz2_0 [3] ,f loat  V1_0 [3] ,f loat  V2_0[3], 

float  dx0[3],  float  Vl[3], float  V2[3]); 
float  cone2(float  Vl[3],  float  V2[3]); 

void  intercept2 (float  xyzl_0[3],  float  Vl_0[3],  float  xyz2_0[3],  float  V2_0[3], 
float  *rl_error,  float  T2_e_z [num_zeta+l] [num_eta+l] , 
float  x_s[num_zeta+l] [num_eta+l] ,  float  y_s [num_zeta+l] [num_eta+l] , 
float  z_s [num_zeta+l] [num_eta+l] ,  float  xyz_T2_max [ 3 ] , 
float  accel_l  [nuitL.zeta+1]  [num_eta+l]  , 
float  accel_2 [num_zeta+l] [num_eta+l] ) ; 
void  cross__prod (float  cross [3],  float  vecl[3],  float  vec2[3]); 

LONG  WINAPI  WndProc  (HWND,  UINT,  WPARAM,  LPARAM) ; 
void  SetDCPixelFomat  (HDC); 
void  InitializeRC  (void); 

void  Dra??Sur face  (float  x_s  [num_zeta+l]  [num_eta+l] ,  float  y_s[num__zeta+l]  [num_eta+l] , 

float  z_s [num_zeta+l] [num_eta+l] ,  float  T2_e_z [num_zeta+l] [num_eta+l] , 
float  accel_l [nura__zeta+l] [num_eta+l] ,  float  accel2 [num_zeta+l] [num^eta+l] , 
float  accel_l_limit ,  float  accel_2_limit) ; 

//void  DrawScene  (HDC  hdc,  float  vert_angle,  float  horz_angle); 
void  DrawScene  (HDC  hdc); 

HPALETTE  hPalette  -  NULL; 


//const  int  textcolor [ 16]  »  {C0L0R_WIND0ViTEXr};  //  used  by  SetSysColor  to  get  black  text 

//const  int  bkcolor[16]  *  {C0L0R_WIND0W};  //  used  by  SetSysColor  to  get  white  background 

//const  COLORREF  blackcolor[3]  «  {RGB(0,0,0)};  //used  by  SetSysColor  to  get  black  text 
//const  COLORREF  whitecolor [3] *{RGB (255,255,255) };//  used  by  SetSysColor  to  get  white  background 

float  x__s [num_zeta+l] [num_eta+l] ;  //  x  coord  of  point  on  intercept  surface 

float  y_s[num_zeta+l] [num_eta+l] ;  //  y  coord  of  point  on  intercept  surface 

float  z_s [num_zeta+l] [num_eta+l] ;  //  z  coord  of  point  on  intercept  surface 

float  T2_e_z [num_zeta+l] [num_eta+l] ; 

float  accel_l [num_zeta+l] [num„eta+l] ,  accel_2 [num_zeta+l] [num_eta+l] ;//  accel  of  veh  1  and  2 
float  accel_l_limit ,  accel_2_limit ; 

float  xyz_T2_max [ 3 ] ;  /*  location  on  surface  with  max  intercept  time  */ 

float  rl_error  -  O.lf;”/*  max  error  in  computed  dist  from  aircraft  to  surface  */ 

float  eta2_kl; 


/*  The  following  temporary  variables  are  used  to  update  the  state  variables  */ 
float  Time  «  O.Of,  dT  -  .5f; 
float  center 1[ 3],  vl_0; 
float  center2[3],  v2_0; 

float  crossl[3],  dist_sql,  radiusl,  Wl[3],wl,  rad_vecl [3] ,rad_vecl_[3] ,  csl,ssl; 
float  cross2[3],  dist_sq2,  radius2,  W2[3],w2,  rad_vec2 [3] ,rad_vec2_ [3] ,  cs2,ss2; 


/*  The  following  "state"  variables  get  updated  when  time  advances  */ 

// - 

// 


/* 
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//This  data  is  planar  and  gives  a  surface  that  just  touches  the  cone 
float  xyzl_0[3]  *  {  O.OOOf,  O.OOOf,  O.OOOf}  ;  //  position  of  aircraft 

//float  xy22_0[3]  *  {-4 . 603f ,-8 . 876f ,  2.954f};  //  position  of  missile 

float  xyz2_0[3]  -  {-4 . 603f* 1 . 4f . 876f* 1 . 4f ,  2 . 954f*l .4f };  //  position  of  missile (scale) 
float  Vl_0[3]  -  {  1.234f,  -.593f,  .139f};  //  velocity  of  aircraft 

float  V2„0[3]  -  {-4.199f,  2.961f,  -.761f};  //  V2  >>  VI  to  allow  intercept 

float  mid_point[3]  -  {-2.f,  -4.f,  1.5f};  //  center  between  xyzl_0  and  xyz2_0  INITIAL 
// 

float  vert_angle  »  -50. Of;  //  degrees  rotate  view  about  vertical  axis 
float  horz^angle  *  2 10. Of;  //  degrees  rotate  view  about  horizontal  axis 
//  limits 

float  accel_l_limit  -  1.4f;  //  2*V1*V1/1 rl-r2 1 
float  accel_2_limit  -  4. Of;  //  2*V2*V2/j rl-r2 | 

float  d_accel  »  .If;  //  changes  in  accel  limits  when  F7  and  F8  key  used 

*/ 

// 

// - 

//  This  data  in  the  Ecalm  Final  Report,  December  1995 
float  xyzl_0[3]  *  {  O.Of,  O.Of,  O.Of};  //  position  of  aircraft 
float  zyz2„0[3]  -  {  6. Of, -5. Of,  4. Of};  //  position  of  missile 
float  Vl_0[3]  -  {  -.3f,  --If,  -.If);  //  velocity  of  aircraft 

float  V2_0[3]  -  {  -.2f,  .Of,  -.6f};  //  V2  >>  VI  to  allow  intercept 

float  mid^, point [ 3 ]  *  {3.f,  -2.5f,  2.f};  //  center  between  xyzl_0  and  xyz2_0  INITIAL 
// 

float  vert^angle  -  -20. Of;  //  degrees  rotate  view  about  vertical  axis 

float  horz_angle  »  20.0f;  //  degrees  rotate  view  about  horizontal  axis 

//  limits 

float  accel_l_limit  «  .If;  //  2«V1*V1/| rl-r2 | 
float  accel_2_limit  -  .15f;  //  2»V2*V2/! rl-r2 | 

float  d_accel  -  .02f;  //  changes  in  accel  limits  when  F7  and  F8  key  used 

// - - - ; - 

/* 

//  This  data  gies  an  accel2_limit  region  with  a  hole  in  it 
float  xyzl_0[3]  »  {  O.Of,  O.Of,  O.Of};  //  position  of  aircraft 
float  xyz2_0[3]  -  {  6 . 0f*2 . Of ,-5 . 0f*2 . Of ,  4.0f*2.0f};  //  position  of  missile 

float  Vl_0[3]  -  {  -.30f,  .25f,  -.20f};  //  velocity  of  aircraft 

float  V2_0[3j  -  {  -.66f,  .51f,  -.44f};  //  V2  >>  VI  to  allow  intercept 

float  mid_point[3]  «  {3.f,  -2.5f,  2.f};  //  center  between  xyzl_0  and  xyz2_0  INITIAL 
// 

float  vert_angle  -  -20. Of;  //  degrees  rotate  view  about  vertical  axis 
float  horz_angle  -  -30. Of;  //  degrees  rotate  view  about  horizontal  axis 
//  limits 

float  accel_l_limit  -  0.220f/2.0f;  //  2*V1*V1/| rl-r2 | 
float  accel_2_limit  -  0.053f/2.0f;  *//  2*V2*V2/| rl-r2 | 

float  d_accel  -  .002f;  //  changes  in  accel  limits  when  F7  and  F8  key  used 

*/ 

// - - - 

/* 

//  This  data  needs  more  grid  points  or  more  iterations  to  converge,  120x120  grid,  4  iterations 
//  NONplanar  (almost  3  orthog  vectors)  and  gives  a  surface  that  just  touches  the  cone 
float  xyzl_0[3]  -  {  O.Of,  O.Of,  O.Of};  //  position  of  aircraft 
//float  xyz2_0[3]  «  {  0.2f,  6. Of, -4. Of};  //  position  of  missile 

float  xyz2_0[3]  *  {  0.4f,  12 . Of ,-8 . Of };  //  position  of  missile  (x2-xl  stretches  everthing) 

//float  Vl_0[3]  -  {  2.294f,  .20f,  .20f};  //  velocity  of  aircraft 
float  Vl_0[3]  -  {  2.290f,  .20f,  .20f};  //  velocity  of  aircraft 

float  V2_0[3]  -  {  .20f,  4.00f,  6.00f};  //  V2  >>  VI  to  allow  intercept 

float  mid_point[3]  -  {O.f,  3.f,  -2.f};  //  center  between  xyzl„0  and  xyz2_0  INITIAL 

// 

float  vert_angle  -  -  60. Of;  //  degrees  rotate  view  about  vertical  axis 

float  horz_angle  -  120. Of;  //  degrees  rotate  view  about  horizontal  axis 

//  limits 

float  accel_l_limit  -  3. Of;  //  2*V1*V1/| rl-r2 | 
float  accel_2_limit  »  8. Of;  //  2*V2*V2/i rl-r2 j 

float  d_accel  -  .If;  //  changes  in  accel  limits  when  F7  and  F8  key  used 

•/ 

// - 

int  big_cone  -  0;  //  initialize  with  no  cone  drawn  (Home  key  turns  on  cone  etc.) 
int  sign  -  1;  //  "END"  key  changes  this  to  -1  to  flip  sign  on  keyboard  entry. 

float  dx0[3];  //initial  offset 

float  Vl[3];  //  initial  velocity  of  aircraft 

float  V2[3];  //  initial  velocity  of  missile  2 


//  For  changing  VI  so  solution  components  just  touch. 

float  V2_unit[3],  V2_per[3],  d_r_unit[3],  d__r[3],  tmp[3],  Vl_dir[3],  VLunit[3]; 

float  v2,  los,  vl_over_v2,  eta2c.  r2,  V2_dot_r,  Vl_dir_norm^  delta_r,  cos_eta2c,  sin_eta2c; 

float  dlos,  cos_dlos,  sin_dlos;  //  to  change  los,  by  changing  V2_0 
float  drl_kiss,  rl_kiss[3];  //  to  plot  the  (ellipsoid,  cone)  kiss  point 
// - 

/* 

•  Function  WinMain. 

*/ 


int  WINAPI  WinMain 
{ 


(HINSTANCE  hinstance,  HINSTANCE  hPrevInstance , 
LPSTR  IpszCmdLine,  int  nCmdShow) 


static  char  szAppNamef]- 

"Intercept  Surface:  aircraft  at  'apple  stem',  black  ball  at  max-time  intercept"; 
WNDCLASS  wc; 

UiM)  hwnd;  .  : 

MSG  msg ; 

wc. style  -  CS_HREDRAW  |  CS_VREDRAW;  //  Horizontal  or  Vertical  redraw? 

wc . IpfnWndProc  -  (WNDPROC)  WndProc; 

wc.cbClsExtra  0; 

wc . cbWndExtra  «  0; 

wc.hinstance  »  hinstance; 

wc.hicon  -  Loadicon  (NULL,  IDI^APPLICATION) ; 
wc.hCursor  -  LoadCursor (NULL ,  IDC_ARR0W); 

wc.hbrBackground  «  (HBRUSH)  (C0L0R_WIND0W  +1);  //  overwritten  by  glClearColor  later 

//wc . hbrBackground  -  (WHITE_BRUSH) ;  //  overwritten  by  glClearColor  later 

//wc.hbrText  -  (HBRUSH)  (COLOR^WIOTOWTEXT  +1);  //  Try  to  make  black  text  ERROR 

wc . IpszMenuName  -  NULL; 

wc . IpszClassName  -  szAppName; 

RegisterClass  (&wc); 

//hwnd  “  CreateWindow  (szAppName,  szAppName, 

//  WS_OVERLAPPEDWINDOW  |  WS^CLIPCHILDREN  1  WS^CLIPSIBLINGS, 

//  CW_USEDEFAULT,  CW_US ED E FAULT,  CW_USEDEFAULT ,  CW^USE DEFAULT,  //random 

//  1M?D_DESKT0P ,  NULL,  hinstance,  NULL); 

hwnd  -  CreateWindow  (szAppName,  szAppName, 

WS^OVERLAPPEDWINDOW  ]  WS_CLIPCHILDREN  ]  WS^CLIPSIBLINGS, 

0,0,800,600,  //  upper_left_corner_xy  and  lower_right_comer_xy 
HWND_DESKTOP ,  NULL,  hinstance,  NULL); 

ShowWindow  (h?md,  nCmdShow) ; 

UpdateWindow  (hwnd); 

initialize_state (xyzl_0,xyz2_0,Vl_0,V2_0,  dxO,Vl,V2);  //  aircraft  and  missile 

//SetSysColors ( 1,  bkcolor,  whitecolor) ;  //  white  background 
//SetSysColors ( 1,  textcolor,  blackcolor) ;  //  black  text 

while  (GetMessage  (&msg,  NULL,  0,  0))  { 

TranslateMessage  (&rasg); 

DispatchMessage  (^sg); 

} 

return  msg.wParam; 


/» 

«  WndProc  processes  messages  to  the  main  window. 

*/ 


LONG  WINAPI  WndProc  (HWND  hwnd,  UINT  msg,  WPARAM  wParam,  LPARAM  IParam) 

{ 

static  HDC  hdc; 
static  HGLRC  hrc; 

PAINTSTRUCT  ps; 

GLdouble  gldAspect; 

GLsizei  glnWidth,  glnHeight; 
static  BOOL  bUp  =  TRUE; 
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static  UINT  nTimer; 
int  n; 


//SetBkColor(  hdc.  GetSysColor  (COLOR^WINDOW)  );  //  see  p  223 
//SetTextColor(hdc ,  GetSysColor  (COLOR_WINDOWriLXT)  );  //  see  p  223 

switch  (msg)  { 


case  WM_KEYDOWN: 

switch  (wParam) 

{ 

/*  Rotate  the  view  */ 
case  VK_LEFT: 

vert^angle  10. Of; 

return  0; 
case  VK_RIGHT: 

vert_angle  +-  10. Of; 
case  VR_UP: 

horz^angle  10. Of; 

return  0; 
case  VRJOWN: 

horz_angle  +-  10. Of; 
return  0; 


case  VK__END : 

sign  «  -sign;  //  Changes  sign  on  other  keyboard  entries 
return  0; 


/»  Change  Vl_  to  make  the  two  solution  components  just  touch  •/ 
case  VK_H0ME : 
if  (  big_cone  *=*  2) 
big_cone  -  0; 
else 
{ 

big_cone  -  2; 

v2  «  (float)  sqrt(V2[0]*V2[0]+V2[l]*V2[l]+V2[2]*V2[2]); 

V2_dot_r  -  V2_0[0]»(xyzl_0[0]-xyz2_0[0])  + 

V2_0[l]*(xyzl_0[l]-xyz2_0[l])  + 

V2_0[2]*(xyzl_0[2]-xyz2_0[2]); 

delta_r  -  (float)  sqrt ( (xyz2_0 [0] -xyzl^O [0] )* (xy22_0[0] -xyzl_0 [0] )  + 

( xyz2_0 [ 1 ] -xyz 1_0 [ 1 ] ) * (xyz2_0 [ 1 ] -xyz 1_0 [  1  ] )  + 
(xyz2_0[2]-xyzl_0[2])*(xyz2_0[2]-xyzl_0[2] )  ); 
los  -  (float)  acos  (V2_dot_r/(v2*delta_r) ) ; 

vl_over_v2  -  .724f*(1.03f  -  (f loat )sin (los/2 . Of ) )  ;//  solution  components  just  merge 

vl_over_v2  *  .724f*(  .99f  -  (float)sin (los/2 .Of )) ;//  solution  components  just  separate 

vl__over_v2  =  .724f»(1.01f  -  (float ) sin (los/2 .Of )) ;//  solution  components  about  kiss 

eta2c  »  (3.14159f  -  (float)  asin (vl_over_v2)  +  los)/2.0f;  //  use  to  draw  cone 

//  iterate  to  get  better  than  2*/*  accuracy  on  max  vl/v2  that  gives  separate  sol  comp 
vl_over_v2  -  (float)  (  (sin (eta2c)/eta2c)  / 

sqrt(  1  +  (l/eta2c  -  1/tan (eta2c) )*( l/eta2c  -  1/tan (eta2c) )  )  ); 

eta2c  »  (3.14159f  -  (float)  asin (vl__over_v2)  +  los)/2.0f; 
vl__over_v2  -  (float)  (  (sin (eta2c)/eta2c)  / 

sqrt (  1  +  (l/eta2c  -  1/tan (eta2c) )*( l/eta2c  -  1/tan (eta2c) )  )  ); 

eta2c  -  (3.14159f  -  (float)  asin (vl_qver_v2)  +  los) /2, Of;  //  use  to  draw  cone 


d_r[0]  -  (xyz2_0[0]-xyzl_0[0] ) ; 
d_r[l]  -  (xyz2_0[l]-xyzl_0[l]); 
d_r[2]  -  (xyz2_0[2]-xy2l_0[2]); 
cross_prod (tmp,  d_r,  V2_0); 

cross_prod (Vl_dir^  d_r^  tmp);//  VI  perp  to  d_r,  in  plane  of  d_r  and  V2_ 

Vl_dir_norm-( float  )sqrt  (Vl_dir [0]*Vl_dir [0]+Vl_dir [  l]*Vl_dir [  1]+Vl_dir [2]*Vl_dir[2] )  ; 

Vl_unit[0]  -  Vl_dir [0]/Vl_dir_norm; 

Vl_unit[l]  -  Vl_dir [ l]/Vl_dir_norm; 

Vl_unit[2]  »  Vl_dir [2]/Vl_dir_norm; 

drl_kiss  -  delta_r  *  (float)  tan(eta2c  -  los); 
rl_kiss[0]  -  drl_kiss*Vl_unit [0] ; 
rl_kiss[l]  =  drl_kiss*Vl_unit [1] ;  4 


rl_kiss[2]  -  drl_kiss*Vl_unit [2] ;  //  use  to  plot  the  (ellipsoid,  cone)  kiss  point 


V1_0[0]  -  vl_over_v2*v2*Vl„unit [0] ; 

V1_0[1]  -  vl_over_v2*v2*Vl_unit [ 1] ; 

Vl_0[2]  -  vl_over_v2*v2*Vl_unit [2] ; 

Time  -  O.Of;  //  Restart  time  (then  reset  the  initial  condition) 

VI [0]  «  V1_0[0];  //  Transfer  the  current  state  to  the  new  initial  condition 
Vl[l]  “  V1_0[1]; 

Vl[2]  -  Vl_0[2]; 

V2[0]  *  V2_0[0]; 

V2[l]  -  V2_0[l]; 

V2[2]  -  V2_0[2]; 
dx0[0]  =  xyz2__0[0]  -  xyzl_0[0]; 

dx0[l]  «  xyz2_0[l]  -  xyzl_0[l]; 

dx0[2]  -  xyz2_0[2]  -  xyzl_0[2]; 

eta2_kl  -  cone2 (V1,V2) ; 

} 

return  0; 

change  the  velocity  of  the  missile  */ 
case  VK_F3: 

V1_0[0]  +“  sign  »  .If;  //  increment  one  element  of  the  current  state 
Time  -  O.Of;  //  Restart  time  (then  reset  the  initial  condition) 

V1[0]  -  V1_0[0];  //  Transfer  the  current  state  to  the  new  initial  condition 
Vl[l]  -  V1_0[1]; 

Vl[2]  -  Vl„0[2]; 

V2[0]  =  V2„0[0]; 

V2[l]  -  V2_0[l]; 

V2[2]  -  V2_0[2]; 
dx0[0]  -  xyz2_0[0]  ~  xyzl_0[0]; 

dx0[l]  -  xyz2_0[l]  -  xyzU[l]; 

dx0[2]  »  xyz2_0[2]  -  xyzl_0[2]; 

eta2_kl  -  cone2 (V1,V2) ; 
return  0; 
case  VK_F4: 

V1_0[1]  +-  sign  *  .If; 

Time  *=  O.Of; 

V1[0]  -  V1_0[0]; 

Vl[l]  -  V1^0[l]; 

Vl[2]  -  Vl_0[2] ; 

V2[0]  -  V2_0[0]; 

V2[l]  «  V2_0[l]; 

V2[2]  -  V2_0[2]; 
dx0[0]  -  xyz2_0[0]  -  xyzl_0[0]; 
dx0[l]  -  xyz2_0[l]  -  xyzl_0[l]; 
dx0[2]  -  xy22_0[2]  -  xyzl_0[2]; 
eta2_kl  =  cone2 (V1,V2) ; 
return  0; 
case  VK_F5 : 

Vl_0[2]  +«  sign  *  .If; 

Time  *  O.Of; 

V1[0]  =  V1.0[0]; 

Vl[l]  »  V1_0[1]; 

Vl[2]  -  Vl_0[2]; 

V2[0]  -  V2_0[0]; 

V2[l]  -  V2_0[l]; 

V2[2]  »  V2_0[2]; 
dx0[0]  -  xyz2_0[0]  -  xyzl_0[0]; 
dx0[l]  -  xyz2_0[l]  ~  xyzl_0[l]; 
dx0[2]  -  xyz2_0[2]  -  xyzl__0[2]; 
eta2_kl  -  cone2 (V1,V2) ; 
return  0; 

case  VK_F6:  //  change  line-of ^sight  angle  from  missile  to  aircraft 
//  compute  OLD  los  angle 

v2  -  (float)  sqrt(V2[0]*V2[0]+V2[l]*V2[l]+V2[2]*V2[2]); 

v2  -  (float)  sqrt(V2[0]*V2[0]+V2[l]*V2[l]+V2[2]*V2[2]); 

V2_dot_r  -  V2_0[0]*(xyzl_0[0]-xyz2_0[0])  + 

V2_0[l]*(xyzl_0[l]-xyz2_0[l])  + 

V2_0[2]*(xyzU[2]-xyz2J5  2]); 


delta_r  «  (float)  sqrt ( (xyz2_0 [0 ] -xyzl_0 [0] )* (xyz2_0 [0] -xyzl^O [0 ] )  + 

( xyz2_0 [ 1 ] -xyz 1_0 [ 1 ] ) * ( xyz  2_0 [ 1 ] -xyz 1^0 [ 1 ] )  + 
(xyz2_0[2]-xyzl.0[2])*(xyz2_0[2]-xy2l_0[2])  ); 
los  =  (float)  acos  (V2_dot__r/(v2*delta_r) ) ; 

//  Compute  unit  vector  perp  to  V2,  in  plane  of  V2  and  (r2-rl) 

V2_unit[0]  -  V2_0[0]/v2; 

V2^unit[l]  =  V2_0[l]/v2; 

V2_unit(2]  *  V2_0[2]/v2; 

d_r_unit [0]  -  (xyzl_0 [0] “Xyz2_0[0] )/delta_r; 
d_r_uni t [ 1 ]  -  (xyz 1_0 [ 1 ] “Xyz2_0 [ 1 ] ) /del ta_r ; 
d_r_unit [2]  -  (xyzl_0 [2] -xyz2_0 [2] )/deita_r; 

V2_per[0]  =  (d_r_unit[0]  ^  (float)cos(los)*V2_unit[0])  /  (float)sin (los 

V2_per[l]  “  (d_r_unit[l]  ~  (float )cos (los)»V2_unit [ 1] )  /  (f loat)sin (los 

V2_per[2]  *  ( d_r__un i t [ 2 ]  -  (f loat )cos (los)*V2_unit [2] )  /  (float )sin (los 

dlos  *  sign  *  .Olf;  //  los  changed  by  the  F6  key 

los  +«  dlos; 

//  Rotate  V2  by  the  increment  in  the  los  angle 
cos_dlos  -  (float)  cos (dlos); 
sin_dlos  »  (float)  sin(dlos); 

V2_0[0]  -  V2_0 [0]*cos_dlos  -  v2*V2_per [0]*sin_dlos; 

V2_0[l]  «  V2_0[l]*cos_dlos  -  v2*V2_per [ l]*sin_dlos; 

V2_0[2]  -  V2_0 [2]*cos_dlos  -  v2*V2_per [ 2]»sin_dlos; 

Time  *  O.Of; 

V1[0]  -  V1_0[0]; 

Vl[l]  «  V1_0[1]; 

Vl[2]  -  Vl_0[2]; 

V2[0]  -  V2_0[0]; 

V2[l]  -  V2_0[l]; 

V2[2]  -  V2_0[2]; 
dx0[0]  »  xyz2_0[0]  -*  xyzl_0[0]; 
dx0[l]  -  xyz2_0[l]  -  xyzl_0[l]; 
dx0[2]  -  xyz2_0[2]  -  xyzl_0[2]; 
eta2_kl  -  cone2  (VUV2) ; 
return  0; 

case  VK_F7 :  /*  change  aircraft  acceleration  limits  */ 
accel_l_limit  -  accel_l_limit  +  sign*d_accel ; 
return  0; 

case  VK_F8:  /*  change  missile  acceleration  limits  */ 
accel_2_limit  »  accel__2_limit  +  sign*d_accel ; 
return  0; 

/*  update  the  state  */ 

case  VR_F1:  /*  missile  and  aircraft  each  on  arcs  to  xyz_T2_max  */ 

Time  +*  dT; 

//  aircraft - // 

vl_0  -(float)  sgrt(Vl_0[0]*Vl_0[0]+Vl_0[l]*Vl_0[l]+Vl_0[2]*Vl_0[2]); 
cross  1[0]  -  V1_0  [  1]*  (xyz__T2_max  [2]  -  xy2l_0[2])  - 
V  1__0  [  2  ] » ( xyz_T2_max  [  1  ]  -  xyz  1_0  [  1  ] )  ; 
cross 1[1]  *  V1_0 [2]* (xyz_T2_max [0]  -  xyzl_0[0])  - 
Vl_0[0]*(xyz_T2.max[2]  -  xyzl_0[2]); 
crossl[2]  -  Vl_0[0]*(xyz_T2_max[l]  -  xyzl_0[l))  - 
V1_0 [ 1]* (xyz_T2_max [0]  -  xyzl_0[0]); 
dist_sql  -  (xyz_T2_max [0]  -  xyzl_0 [0] )* (xyz_T2_max [0]  -  xyzl_0[0])  + 
(xyz_T2_max[l]  -  xyzl_0 [  1]  )* (xyz_T2_max [  1]  -  xyzl_0[l])  + 
(xyz_T2__max [2]  -  xyzl_0 [2]  )* (xyz_T2_max [2]  -  xyzl_0[2]); 
radiusl  -  . 5f*vl_0*dist_sql/  (float) 

sqrt (cross  1 [ 0 ] *cross 1 [ 0 ] +cross 1 [ 1 ] *cross 1 [ 1 ] +cross 1 [ 2 ] *cross 1 [ 2 ] ) ; 
W1[0]  -  V1_0 [ l]*crossl [2]  -  V1_0 [2] *crossl [ 1] ; 

Wl[l]  -  Vl_0[2]*crossl[0]  -  V1_0 [0]*crossl [2 ] ; 

Wl[2]  »  Vl_0[0]*crossl[lj  -  V1_0 [ 1 j*crossl [0] ; 

wl  -  (float)  sqrt(Wl[0]*Wl[0]  +W1[1]*W1[1]  +  W1 [2]*W1 [2] ) ; 

rad_vecl[0]  -  radiusl»Wl [0 ]/wl; 

rad_vecl[l]  -  radiusl*Wl [ l]/wl; 

rad_vecl[2]  -  radiusl*Wl [2]/wl ; 

rad_vecl_[0]  -  radius 1«V1_0 [0]/vl_0; 

rad_vecl_[l]  -  radius  1»V1__0  [  l]/vl_0; 

rad_vecl_[2]  -  radius  1*V1__0 [2]/vl_0;6 


csl  -  (float)  cos (vl_0*dT/radiusl) : 
ssl  -  (float)  sin (vl_0*dT/radiusl) ; 

centerl[0]  =  xyzl_0[0]  -  rad_vec 1 [ 0 ] ;  //  center  of  circle 
centerl[l]  «  xyzl_0[l]  -  rad_vecl[l]; 

centerl[2]  -  xyzl_0[2]  -  rad_vecl[2]; 

xyzl_0[0]  -  center  1[0]  +  rad^vecl [0]*csl  +  rad__vec  1_ [ 0 ] *ss  1 ; 

xyzl_0[l]  -  centerl[l]  +  rad_vecl [ l]»csl  +  rad_vecl_[ l]*ssl; 

xyzl_0[2]  =  centerl[2]  +  rad_vec 1 [ 2 ] *cs 1  +  rad_vecl_[2]*ssl; 

V1_0[0]  »  (vl_0/radiusl)* (-rad_vecl [0]*ssl  +  rad_vecl_[0]*csl) ; 
V1_0[1]  *  (vl_0/radiusl)* (-rad_vecl [ l]*ssl  +  rad_vecl_[ l]*csl) ; 
V1_Q[2]  «  (vl_0/radiusl)* (-rad_vecl [2]*ssl  +  rad_vecl_[2 ]*csl) ; 

//  missile - // 

v2_0  * ( f 1 oat )  sqrt ( V2_0 [ 0 ] *V2_0 [ 0 ] +V2_0 [ 1 ] *V2_0 [ 1 ] +V2_0 [ 2 ] *V2_0 [ 2 ] ) ; 
cross2[0]  -  V2_0[l]*(xyz_T2_max[2]  -  xyz2_0[2])  ~ 
V2_0[2]*(xyz_T2_raax[l]  -  xyz2_0[l]); 
cross2[l]  -  V2_0 [2]* (xyz_T2_max [0]  -  xyz2_0[0])  ~ 
V2_0[0]*(xy2_T2_max[2]  -  xy22_0[2]); 
cross2[2]  »  V2_0[0]*(xyz_T2_max[l]  -  xyz2_0[l])  - 
V2J[l]*(xy2^T2^max[0]  -  xyz2_0[0]); 
dist„sq2  -  (xyz£I7:jaaxfG]  —  -  xyz2_0[0] )  + 

(xyz_T2_max[l]  -  xyz2_0[  1]  )♦  (xy2:_T2_max [  1]  -  xyz2_0[l])  + 
(xyz_T2_max[2]  -  xyz2_0 [2] )* (xy2_T2.max[2]  -  xyz2_0[2]); 
radius 2  =  . 5f*v2_0*dist_sq2/  (float) 

sqrt (cross2 [0]*cross2 [0]+cross2 [ l]*cross2 [ l]+cross2 [2]*cross2 [2] ) ; 
W2[0]  -  V2_0[l]*cross2[2]  -  V2_0[2]*cross2[l] ; 

W2[l]  -  V2_0[2]*cross2[0]  -  V2_0 [0]*cross2 [2] ; 

W2[2]  -  V2_.0[0]*cross2[l]  -  V2_0[ l]*cross2[0] ; 

w2  «  (float)  sqrt(W2[0]*W2[0]  +W2[1]*W2[1]  +  W2[2]*W2 [2] ) ; 

rad_yec2[0]  «  radius2*W2 [0]/w2; 

rad__vec2[l]  -  radius2*W2 [ l]/w2; 

rad_v8c2[2]  -  radius2*W2 [2]/w2; 

rad_vec2^[0]  *  radius2*V2_0 [0]/v2_0; 

rad_vec2_  [  1  ]  -  radius2*V2_0  [  1  ]  Ar2_0 ; 

rad_vec2_[2]  -  radius2*V2_0 [2] A;'2__0; 

cs2  -  (float)  cos (v2_0*dT/radius2) ; 

ss2  -  (float)  sin (v2_0*dT/radius2) ; 

cent0r2[O]  -  xyz2_0[0]  -  rad_v0c2[O];  //  center  of  circle 
center2[l]  «  xy22_0[l]  -  rad_vec2[l]; 

center2[2]  -  xyz2_0[2]  ~  rad_vec2[2]; 

xyz2_0[0]  *  center2[0]  +  rad_vec2 [0]*cs2  +  rad_vec2_[0]*ss2; 

xyz2_0[l]  -  C0nter2[l]  +  rad_vec2 [ l]*cs2  +  rad_vec2_[ l]»ss2; 

xyz2_0[2]  -  center2[2]  +  rad_vec2 [2]*cs2  +  rad_vec2_[2]*ss2; 

V2_0[0j  “  (v2_0/radius2)* (“rad_vec2 [0]*ss2  +  rad_vec2„[0]*cs2) ; 
V2_0[l]  *  (v2__0/radius2)* (-rad_vec2 [ l]*ss2  +  rad_vec2_[ l]»cs2) ; 
V2_0[2]  -  (v2_0/radius2)* (-rad_vec2 [2]*ss2  +  rad_vec2_[2]*cs2) ; 
return  0; 

case  VK_F2:  /*  missile  on  arc  to  xyz_T2_max,  aircraft  flies  straight  */ 


Time  +«  dT; 

//  aircraft - — - - — — - - — — - - - - - // 

xyzl_0[0]  +-  dT*VLQ[0];  . 

xyzl_0[l]  +-  dT*Vl_0[l]; 
xyzl_0[2]  +-  dT*Vl_0[2]; 

//  missile - // 


v2_0  « ( f 1 oa t )  sqrt ( V2_0 [ 0 ] *V2_0 [ 0 ] +V2_0 [ 1 ] *V2_0 [ 1 ] +V2_0 [ 2 ] *V2_0 [ 2 ] ) ; 
cross2[0]  -  V2_0[l]*(xyz_T2_max[2]  -  xy2:2_0(2])  - 
V2_0[2j*(xyz_T2_max[l]  -  xyz2_0[l]); 
cross2[l]  -  V2_0[2]* (xyz_T2_max [0]  -  xyz2_0[0])  - 
V2_0[0]*(xyz_T2_max[2]  -  xyz2_0[2]); 
cross2[2]  *  V2_0[0]*(xyz_T2_max[lj  -  xyz2_0[lj)  - 
V2_0[l]*(xyz_T2_max[0]  -  xyz2„0[0]); 
dist_sq2  -  (xyz_T2_max [0]  -  xyz2_0[0] )• (xyz_T2_max [0]  -  xy22_0[0])  + 
(xyz_T2_max[l]  -  xy22J[l]  )*(xy2_T2_max[l]  -  xyz2_0[l])  + 
(xyz_T2_max[2]  -  xyz2_0 [2] )* (xyz_T2_max [2]  -  xyz2_0[2]); 
radius 2  *  . 5f*v2_0*dist_sq2/  (float) 

sqrt (cross2 [ 0 ] *cross2 [ 0 ] +cross2 [ 1 ] *cross  2 [ 1 ] +cross2 [ 2 ] *cross2 [ 2 ] ) ; 
W2[0]  -  V2_0[l]*cross2[2]  -  V2_0 [2]*cross2 [ 1] ; 

W2[l]  -  V2_0[2]*cross2[0]  -  V2_0 [0]*cross2 [2] ; 

W2[2]  -  V2_0[0]*cross2[l]  -  V2_0[ 1 ]*cross2 [0] ; 

w2  -  (float)  sqrt(W2[0]*W2[0]  +W2[1]*W2[1]  +  W2[2]*W2 [2] ) ; 

rad_vec2[0]  »  radius2*W2 [0]/w2; 

rad_vec2[l]  »  radius2*W2 [ l]/w2; 

rad_v0c2[2]  -  radius2*W2 [2]/w2;  7 


rad_vec2__[0]  -  radius2*V2_0  [0]/v2_0; 
rad_vec2_  [  1  ]  *  raidius2*V2_0  [  1  ]  /v2_0 ; 
rad_vec2_[2]  «  radius2*V2_0 [2] Ar2_0; 
cs2  «  (float)  cos (v2_0*dT/radius2) ; 
ss2  =  (float)  sin (v2_0*dT/radius2) ; 

center2[0]  -  xyz2_0[0]  -  rad_vec2[0];  //  center  of  circle 
center2[l]  -  xyz2_0[l]  -  rad_vec2[l]; 

center2[2]  =  xyz2_0[2]  -  rad_vec2[2]; 

xyz2_0[0]  »  center2[0]  +  rad_vec2 [0]»cs2  +  rad_vec2_[0]*ss2; 
xy22__0[l]  -  center2[l]  +  rad_vec2 [ l]*cs2  +  rad_vec2_[ l]*ss2 ; 
xyz2_0[2]  -  center2[2]  +  rad_vec2[2]*cs2  +  rad_vec2_[2]*ss2; 
V2_0[0]  “  (v2_0/radius2)« (“rad_vec2 [0]*ss2  +  rad_vec2_[0]»cs2) 
V2_0[l]  =  (v2_0/radius2)*(-rad_vec2[l]*ss2  +  rad_vec2_[ l]*cs2) 
V2_0[2]  -  (v2_0/radius2)*(-rad_vec2[2]*ss2  +  rad_vec2_[2]*cs2) 
return  0; 

} 

return  0; 

case  WM_CREATE: 

// 

//  Create  a  rendering  context  and  set  a  timer. 

// 

hdc  *»  GetDC  (hwnd); 

SetDCPixelFormat  (hdc); 
hrc  -  wglCreateContext  (hdc); 
wglMakeCurrent  (hdc,  hrc); 

InitializeRC  (); 

nTimer  «  SetTimer  (hwnd,  1,  50,  NULL);  //  milliseconds 
return  0; 

case  ViW^SIZE: 

// 

//  Redefine  the  viewing  volume  and  vie¥rport  once  the  program 
//  starts  and  again  any  time  the  window  size  changes. 

// 

glnWidth  -  (GLsizei)  LOWORD  (IParam); 

glnHeight  »  (GLsizei)  HIWORD  (IParam); 

gldAspect  -  (GLdouble)  glnWidth  /  (GLdouble)  glnHeight; 

glMatrixMode  (GL^PROJECTION) ; 
glLoadldentity  (); 

gluPerspective  (30.0,  gldAspect,  0.48,  48.0); 
glViewport  (0,  0,  glnWidth,  glnHeight); 
return  0; 

case  WM^PAINT: 

// 

//  Draw  the  scene . 

// 

BeginPaint  (hivnd,  &ps); 

//DrawScene  (hdc,  vert_angle,  horz_angle); 

DravyScene  (hdc); 

EndPaint  (hwnd,  &ps); 
return  0; 

case  WM_TIMER: 

// 

//  force  a  repaint. 

// 

InvalidateRect  (hwnd,  NULL,  FALSE); 
return  0; 

case  WM^QUERYNEWPALETTE : 

// 

//  If  the  program  is  using  a  color  palette,  realize  the  pallette 
//  and  update  the  client  area  when  the  window  receives  the  input 
//  focus. 

// 

if  (hPalette  !-  NULL)  { 

if  (n  »  RealizePalette  (hdc)) 

InvalidateRect  (hwnd,  NULL,  FALSE); 
return  n; 


} 
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break ; 


case  ViW_PALETrECHANGED: 

// 

//  If  the  program  is  using  a  color  palette,  realize  the  palette 
//  and  update  the  colors  in  the  client  area  when  another  program 
//  realizes  its  palette. 

// 

if  ( (hPalette  I-  NULL)  &&  ((HVM))  wParam  I-  hwnd))  { 
if  (RealizePalette  (hdc)) 

UpdateColors  (hdc); 
return  0; 

} 

break; 

case  WM_DESTR0Y : 

// 

//  Clean  up  and  terminate. 

// 

wglMakeCurrent  (NULL,  NULL); .  „ 

wglDeleteContext  (hrc); 

ReleaseDC  (h?md,  hdc); 
if  (hPalette  NULL) 

DeleteObject  (hPalette) ; 

KillTimer  (hwnd,  nXimer) ; 

PostQuitMessage  (0) ; 
return  0; 

} 

return  DefWindowProc  (hwnd,  msg,  wParom,  IParom) ; 


*  SetDCPixelFormat  sets  the  pixel  format  for  a  device  context  in 

*  preparation  for  creating  a  rendering  context. 

» 

*  Input  parameters: 

*  hdc  «  Device  context  handle 

« 

*  Returns : 

*  Nothing 


void  SetDCPixelFormat  (HDC  hdc) 

{ 

HANDLE  hHeap; 
int  nColors,  i; 

LPLOQPALETTE  IpPalette; 

BYTE  byRedMask,  byGreenMask,  byBlueMask; 


static  PIXELFORMATDESCRIPTOR  pfd  -  { 
sizeof  (PIXELFORMATDESCRIPTOR), 
1, 

PFD_DRAW_TO_WINDOW  [ 
PFD^SUPPORT^OPENGL  | 
PFD^DOUBLEBUFFER, 

PFD_TYPE_RGBA, 

24, 

0,  0,  0,  0,  0,  0, 

0,  0, 

0,  0,  0,  0,  0, 

32, 

0, 

0, 

PFD^MAIN^PLANE, 

0, 

0,  0,  0 


//  Size  of  this  structure 
//  Version  number 

//  change  to  PFD__DRAW_TO_BITMAP  (to  print) 


//  RGBA  pixel  values 
//  24  bit  color 

//  Rbits  Rshift,  Gbits  Gshift,  Bbits  Bshift 

//  No  alpha  buffer 

//  No  accumulation  buffer 

//  32-bit  depth  buffer 

//  No  stencil  buffer 

//  No  auxiliary  buffer 

//  Layer  type 

//  Reserved  (must  be  0) 

//  No  layer  masks 


int  nPixelFormat; 

hPixelFormat  -  ChoosePixelFormat  (hdc,  &pfd); 
SetPixelFormat  (hdc,  nPixelFormat,  &pfd); 
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DescribePixelFormat  (hdc,  nPixelFormat ,  sizeof  (PIXELFORMATDESCRIPTOR) ,  &pfd); 


if  (pfd.dwFlags  &  PFD^NEED^PALETTE )  { 

nColors  -  1  <<  pfd .cColorBits; 

hHeap  ==  GetProcessHeap  ( ) ; 

(LPLOGP ALETTE)  IpPalette  «  HeapAlloc  (hHeap,  0^ 

sizeof  (LOGPALETTE)  +  (nColors  *  sizeof  (PALETTEENTRY) ) ) ; 

lpPalette->palVersion  -  0x300; 

lpPalette->palNumEntries  »  nColors; 

byRedMask  »  (1  <<  pfd .cRedBits)  -  1; 

byGreenMask  -  (1  <<  pfd .cGreenBits)  -  1; 

byBlueMask  **(!<<  pfd .cBlueBits)  -  1; 

for  (i»0;  i<nColors;  i++)  { 

lpPalette->palPalEntry[i] .peRed  - 

(((i  >>  pfd.cRedShift)  &  byRedMask)  *  255)  /  byRedMask; 
lpPalette~>palPalEntry[i] .peGreen  - 

(((i  >>  pfd.cQreenShift)  &  byGreenMask)  *  255)  /  byGreenMask; 
lpPalette->palPalEntry [i ] .peBlue  - 

(((i  >>  pfd.cBlueShift)  &  byBlueMask)  *  255)  /  byBlueMask; 
lpPalette->palPalEntry[i] .peFlags  »  0; 

} 

hPalette  -  CreatePalette  (IpPalette); 

HeapFree  (hHeap,  0,  IpPalette); 

if  (bPalette  ! -  NULL)  { 

SelectPalette  (hdc,  hPalette,  FALSE); 

RealizePalette  (hdc) ; 

} 

} 

}  //  end  of  SetDCPixelFormat 


/» 

*  InitializeRC  initializes  the  current  rendering  context 

« 

*  Input  parameters: 

*  None 

» 

*  Returns : 

»  Nothing 

*/ 


void  InitializeRC  (void) 

{ 

GLfloat  glfLightAmbient []  «  {  0,lf,  O.lf,  O.lf,  l.Of  }; 

GLfloat  glfLightDiffuse[]  •  {  0.7f,  0.7f,  0.7f,  l.Of  }; 

GLfloat  glfLightSpecular[ ]  -  {  O.Of,  O.Of,  O.Of,  l.Of  }; 

// 

//  Initialize  state  variables. 

// 

glFrontFace  (GL_CCW) ; 

//glCullFace  (GLJACK) ;  //  eliminate  lighting  of  the  back  side  of  the  polygon 
//glEnable  (GL_CULL_FACE) ; 

glDepthFunc  (GL^LEQUAL); 
glEnable  (GL_DEPIH_TEST) ; 

/*  The  following  two  lines  allow  missile  and  cone  to  be  transparent. 

But  the  grphics  slow  down  considerable,  and  the  blendinhas  a  few  glitches 
glEnable  (GLJLEND);  //  for  alpha  blending 
glBlendFunc  (GL^SRC^ALPHA,  GL_ONE_MINUS_SRC_ALPHA) ; 

glClearColor  (l.Of,  l.Of,  l.Of,  O.Of);  //  set  background  color  to  white 
//glClearColor  (  .8f,  .8f,  .8f,  O.Of);  //  set  background  color  to  white(grey) 

// 

//  Add  a  light  to  the  scene. 

// 

glLightfv  (GL^LIGHTO,  GL_AMBIENT,  glfLightAilOient ) ; 


glLightfv  (GL^LIOTTO,  GLJIFFUSE,  glfLightDif fuse) ; 
glLightfv  (GL_LIGHT0,  GL^SPECULAR,  gl fLightSpecular) ; 
glEnable  (GL^LIGOTING) ; 
glEnable  (GL.LIGKTO); 


/* 

»  Dra?fSurface  draws  a  surface  made  of  4  cornered  polygons 


Input  parameters: 
r l_e_2 [ num_2eta+ 1 ] [ num_e ta+ 1 ] 


rl_e__z  [i~0]  [  j  ~0] 
rl_e_2[i-0 ] [j“l] 

[j’l] 

rl_e_2[i-l] [j-0 j 


an  array  of  points  defining  the  surface 


Coordinates  of  first  comer  of  i,j  polygon 
==  Coordinates  of  second  comer  of  i,.j  polygon 
«  Coordinates  of  third  comer  of  i,j  polygon 
-  Coordinates  of  fourth  corner  of  i,j  polygon 


*  Returns: 

*  Nothing 

*/ 

void  DrawSurface  (float  x^{iHm^2eta+l]-[nuffl-!aita+l^-#-f4oat  y-sfnum^zeta+1]  [num_eta+l] , 

float  z_s [num_2eta+l] [num_eta+l] ,  float  T2„e_z [num_zeta+l] [num_eta+l] , 
float  accel_l[num_zeta+l] [num_sta+l] ,  float  accel2 [num_zeta+l] [num_eta+l] , 
float  accel_l_limit,  float  acc0l_2_iimit ) 

{ 

int  i,j; 

float  20,y0,z0,  xl,yl,zl,  x2,y2,z2,  x3,y3,z3; 

GLfloat  glfColor[4]; 


for  (j*l;  j<-num_eta;  j++) 

{ 

for  (i-1;  i<-num_zeta;  i++) 

{ 

xO  -  x_s[i3[j]; 
yo  -  y_s[i][j]; 
zO  -  z.s[i][j]; 

xl  -  x_s[i]  [j-1] ; 
yl  -  y^s[i] [j-1]; 
zl  «  z_s[i]  [j-1] ; 

22  -  x_s[i-l] [j-1] ; 
y2  -  y_s[i-i][j-l]; 
z2  «  z_s[i-l] [j-1]; 

x3  -  x_s[i-l] [j ] ; 
y3  -  y_s[i-l] [j]; 
z3  -  z_s[i-l] [j ] ; 


l.Of; 
O.Of  ; 
O.Of ; 
l.Of; 


//for(ic«0;  ic< 

//gl f Color [0] 
glfColor[0]  * 
glfColor[l]  - 
glfColor[2]  - 
glfColor[3]  - 
if  (accel_l[i] [j ] 
if  (accel_2[i] [j ] 
if  (T2„e^z[i][j] 
{glfColor[0]  - 
glfColor[l]  - 
glfColor[2]  - 
glfColor[3]  « 

} 

glMaterialfv  (GL^FRONT 


ic++)  glfColor[ic]  »  glfColors [ 1] [ic] ; 

.  4f*T2_e_z  [i-l]  [ j  -  l]/r2_e_2[num_zeta/2]  [num_eta/2]  ;/*  Red 

/*  Red  */ 


*/ 


>  accel_l_limit ) 

>  accel_2_limit ) 
—  O.Of) 

O.Of; 

O.Of; 

O.Of; 

O.Of;//  Transparent, 


glfColor[l] 

glfColor[2] 


/«  Green  */ 

/*  Blue  */ 

l.Of;  //  aimraft  accel 
l.Of;  //  missile  accel 


limit 

limit 


because  surface  point  didn't  converge 


GL_.AMBIENr_ANDJIFFUSE,  glfColor); 


glBegin  (GL^POLYGON) ; 

if  (j  <  num_eta)  //  so  2  vectors  below  are  both  nonzero 

//  normal  -  (xl,yl,2l)- (x0,y0,z0)  cross  (x3,y3,z3) - (x0,y0,z0) 
glNormal3f ( (yl-yO)* (z3-z0)  -  (zl-zO)* (y3-y0) , 

(2l-z0)»(x3-x0)  “  (xl-x0)*(z3-z0) , 

(xl-x0)*(y3-y0)  -  (yl-yO)* (x3-x0)  ); 
else  //  (x3,y3,z3)  -  (xQ,y0,20) 

//normal  -  (xl,yl,zl) - (x0,y0,z0)  cross  (x2,y2,z2) - (x0,y0,z0) 
glNormal3f ( (yl-yO)* (z2-z0)  -  (zl-zO  Il(y2-y0) , 


f 
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(z1-20)*(x2-x0)  -  (xl-x0)*(z2-z0), 
(xl~xQ)*(y2-yO)  -  (yl-yO)* (x2-x0)  ); 
glVertex3f (x0,y0,20) ; 
g 1 Vertex 3 f (xl,yl,zl) ; 
glVertex3f (x2,y2^z2) ; 
glVertexSf (x3,y3,23) ; 
glEnd  (); 

}  /*  end  of  i  loop 
}  /*  end  of  j  loop 

glEnable (GL_NORMALIZE) :  //  To  make  the  surface  normals  length  1 

}  /*  end  of  function  */ 


/* 

•  DrawScene  uses  OpenGL  commands  to  draw  the  missile  and  intercept  surface 

• 

*  Input  parameters: 

*  hdc  -  Device  context  handle 

*  vert_angle  -  vertical  viewing  angle  (up/down  arrow  keys) 

*  horz_angle  -  horizontal  viewing  angle  (left/right  arrow  keys) 

« 

*  Returns: 

*  Nothing 

*/ 


//void  DrawScene  (HDC  hdc,  float  vert_angle,  float  horz_angle) 
void  DrawScene  (HDC  hdc) 

{ 

int  ic,  buf fer_length,  fin_i; 

char  buffer[200];  //  for  printing  to  screen 

GLUquadricObj  *glquad;  //  for  the  missile  cylinder  ,  , 

float  v2_0;  //  length  of  V2_0[] 

float  ci,  si,  vl,  v2; 

float  los,  dot_prod,  delta_r; 

float  missile_length,  cone_length; 

float  text_xmin  -  -l.Of;  //  corners  of  black  background  box  to  put  text  on 
float  text_xmax  -  l.Of; 
float  text_ymin  -  -l.Of; 
float  text__ymax  -  -.5f; 


GLf loat 

glfBlue[]  -  {O.Of 

,  O.Of, 

l.Of, 

l.Of  }; 

GL float 

glfYellow[]  -  {1. 

Of,  l.Of,  O.Of,  l.Of  }; 

GLf loat 

glfColor [4] ; 

GLf loat 

glfColors[0] [4]- 

{{l.Of, 

O.Of, 

O.Of,  0.5f}, 

{O.Of. 

l.Of, 

O.Of,  0.5f}, 

{O.Of. 

O.Of, 

l.Of,  0.5f}, 

{O.Of. 

l.Of, 

l.Of,  0.5f}, 

{l.Of, 

O.Of, 

l.Of,  0.5f}, 

{l.Of. 

l.Of, 

O.Of,  0.5f}, 

{l.Of, 

l.Of, 

l.Of,  0,5f}, 

{O.Of, 

O.Of, 

O.Of,  0.5f}}; 

// 

//  Clear  the  color  and  depth  buffers, 

// 

glClear  (GL_COLOR_^BUFFER_BIT  |  GLJEPIHJUFFERJIT) ;  //  black  background  ???? 

glMatrixMode  (GL_MODELVIEW) ; 
glLoadldentity  (); 
glTranslatef  (O.Of,  O.Of,  -2. Of); 

// - - - - - 

// 

//  Write  TEXT  and  NUMBERS  to  screen 
// 

glPushMatrixO ; 

glPushAttrib(GL_CURRENTJIT);//  Prevent  cursor  from  being  advanced  each  draw 

glfColor[0]  -  0 . Of ;glfColor [ 1]  -  0 .Of ;glfColor[2]  -  0 . Of ;glfColor [3]  -  O.Of; 
glMaterialfv  (GL^FRONT,  GL__AMBIENT_AND_DIFFI12i,  glfColor); 


glBegin  (GL_POLYGON) ;  //  bldck  background  to  draw  white  text  on 

glVertexSf (text_xmin ,  text^ymin,  O.Of); 
glVertex3f (text_xmin,  text_ymax,  O.Of); 
glVertex3f (text_xmax,  text_ymax,  O.Of); 
glVertexSf (text^xmax^  text_ymin,  O.Of); 
glEnd  (); 


//  SetTextColor(hdc,  RGB (255,0,0) ) ;  //  RGB(255,255,255)  -white  DOESN'T  WORK 

//  SelectObject (hdc,  GetStockObject (BLACKJiklSH) ) ;//  try  to  make  black  text.  DOESN'T  WORK 

//SetSysColors(l,  textcolor,  blackcolor) ;  //  black  text,  PUT  OUTSIDE  LOOP 

//SetBkColor (  hdc,  GetSysColor  (COLOR__WINDOW)  );  //  see  p  223 

//SetTextColor (hdc,  GetSysColor  (COLOR^WINDOWTEXT)  );  //  see  p  223 

SelectObject (hdc,  GetStockObject (SYSTEM_FONT) ) ; 

wglUseFontBitmaps (hdc,  0,  255,  1000) ;//  Start  at  0,  255  Glyphs,  offset  1000 
glListBase(lOOO) ; 

vl  «  (float)  sqrt(Vl[0]»Vl[0]+Vl[l]*Vl[l]+Vl[2]*Vl[2]); 
v2  -  (float)  sqrt(V2[0]*V2[0]+V2[l]*V2[l]+V2[2]*V2[2]); 
dot_prod  -  V2_0[0]*(xyzl_0[0]-xy22_0[0] )  + 

V2_0[l]*(xyzU[l]-xyz2_0[l])  + 

V2.0  [  2  ]  *  (xyz  1^0  [  2  ] -xyz2_0  [  211 ; 

delta^r  »  (float)  sqrt  ( (xyz2_0[0]-xyzl_0[0]  )*(xy22_0[0]-xyzl_0[0] )  + 

(xyz2_0[l]-xyzl_0[l])*(xyz2_0[l]-xyzl_0[l])  + 
(xyz2_0[2]-xyzL0[2])*(xyz2_0[2]-xyzl_0[2])  ); 
los  -  (float)  acos (dot_prod/(v2*delta_r) ) ; 
buf fer^length  -  sprintf (buffer, 

"Time-5C4.1f  x2  (0) -xl  (0)  -  (5;4 .  If  ^4. If  5;4.1f)  VI  (0)  -  (5i4 .  If  ?i4.1f  5^4. If)  V2  (0)«  (5;4 .  If  %4,lf 

^4.  If)  v2/vl-Ji4.2f  >?  Z4.2£  error-?C4 . 2g " , 

Time,  dx0[0],dx0[l],dx0[2],  VI [0] ,V1 [ 1] ,V1 [2] ,  V2 [0] ,V2 [ 1] ,V2 [2] ,  v2/vl,  1 .0f/( .724f* ( 1 . Olf-s 
in (los/2 . Of ) ) ) ,  rl_error) ; 

//glColorSf (0 ..Of ,  O.Of,  O.Of);  //  tried  to  make  text  black 
//glTranslatef  (O.Of, 12. Of,  O.Of);  //  locate  cursor  DOESN'T  WORK 

glCallLists(buffer_length,  GL_UNSIGNEDJYrE,  buffer); 


glPopAttrib ( ) ; 
glPopMatrix ( ) ; 

/* - ,/ 

// 

//  Position  the  model  relative  to  the  viewpoint. 

// 

glTranslatef  (O.Of,  O.Of,  -35. Of);  //  move  the  origen  away  from  viewer,  into  the  screen 
glRotatef  (horz^angle,  l.Of,  O.Of,  O.Of); 
glRotatef  (vert_angle,  O.Of,  l.Of,  O.Of); 

glTranslatef  (-mid_p6int [0] ,  -mid_point [ 1] ,  -mid_point [2] ) ;  //  center  midpoint 

// 

//  Draw  the  missile 
// 

glPushMatrixO  ; 

glTranslatef (xyz2_0 [0] ,  xyz2_0[l],  xyz2„0[2])  ;  //  location  of  missile 

vl_0  -  (float)  sqrt(Vl_0[0]*Vl_0[0]+VO[l]*Vl_0[l]+Vl_0[2]*Vl_0[2]);//  v2/vl  printed 

v2_0  -  (float)  sqrt(V2_0[0]*V2_0[01+V2_0[l]*V2_0[l]+V2J[2]*V2_0[2]); 

glRotatef(  (float)  (-( 180/pi )*atan2 (V2_0 [ 1] ,V2_0 [2] )) ,  l.Of,  O.Of,  O.Of);//  -phi 

glRotatef (  (float)  (  ( 180/pi )*asin (V2_0 [0]/v2_0) ) ,  O.Of,  l.Of,  O.Of);  //  -theta 

//  Now  missile  along  z  axis  (tail  at  xyz2_0) 

missile_length  «  1.5f; 

cone_length  -  15. Of; 

glTranslatef (O.Of ,  O.Of,  - (missile_length  +  .6f));  //  length  (so  nose  at  xyz2_0) 

// - 

//  fins 

for(ic-0;  ic<-3;  ic++)  glfColor[ic]  -  glfColors [ 1] [ic] ; 
glMaterialfv  (GL_FR0NT,  GL_AMBIENr_AND_DIFFUSE,  glfColor); 
for  (fin_i*0;  fin_i<3;  fin_i++) 

{ 

ci  -  (float)  cos (2*pi*f in_i/3) ; 
si  -  (float)  sin (2*pi*f in_i/3) ; 
glBegin  (GL_P0LYG0N) ; 


glNormal3f (si , 

ci , 

O.Of; 

l; 

glVertex3f (ci» 

.2f, 

-si* . 

.2f, 

.Of); 

glVertexSf (ci» 

.4f, 

-si* . 

.4f, 

.Of); 

glVertex3f (ci* 

.4f, 

-si*. 

.4f, 

.5f); 

glVertexSf (ci* 

.2f, 

-si* . 

.2f. 

.7f); 

glEnd  (); 

}  /»  end  of  fin_i  loop  */ 
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// - 

for(ic*0;  ic<»3;  ic++)  glfColor[ic]  «  glfColors [4] [ic] ; 
glMaterialfv  (GL^FRONT,  GL_AMBIENT_AND__DIFFUSE ,  glfColor) ; 
glquad  -  gluNewQuadric ( ) ; 

// 

gluCylinder(glquad,  .2,  .2,  missil0_length,  12, 1) ;//B0DY:  name, rad l,rad2, length, nDiv, stacks 
// 

gluQuadricOrientation (glquad,  GLU_INSIDE) ; 
gluDisk (glquad ,  .0,  .2,  12,  1);  //TAIL 

glTranslatef (0 . Of ,  O.Of,  missile^length) ; 
gluQuadricOrientation (glquad ,  GLU_0UTSIDE) ; 
gluCylinder(glquad,  .2,  .0,  .6,  12,  1);  //NOSE 
glTranslatef (0 .Of ,  O.Of,  .6f);  //  origen  is  back  at  xyz2_0 
// 

gluDeleteQuadric (glquad) ; 

// - 

glquad  «  gluNewQuadric () ; 
if  (big_cone  1) 

{  //  draw  cone  with  eta2  such  that  (vl/v2)  •  sin(eta2)/eta2 
if  (eta2_kl  >  1.5708)  //  so  half-cone  is  pointing  in  V2  direction 

{ 

glTranslatef (O.Of ,  O.Of,  -cone_length  ); 

gluCylind8r(glquad,cone_length*tan (-eta2_kl) ,0,cone_length,24, 1) ; 
glTranslatef (0 . Of ,  O.Of,  cone_length  ); 

} 

else 

{ 

gluCylinder (glquad, 0,  cone_length*tan (eta2_kl) ,  cone_length,24, 1) ; 

} 

} 

if  (big_cone  2) 

{  //  draw  cone  with  eta2  such  that  (vl/v2)  -  (sin (eta2)/0ta2)*sin (eta2  -  los) 
if  (eta2c  >  1.5700)  //  so  half-cone  is  pointing  in  V2  direction 

{ 

glTranslatef (0 . Of ,  O.Of,  -cone_length  ); 

gluCyliiider (glquad, cone_length»tan (-eta2c) ,0,cone_length,24, 1) ; 
glTranslatef (O.Of ,  O.Of,  cone_length  ); 

} 

else 

{ 

gluCylinder(glquad,0,cone_length*tan (eta2c) ,cone_length,24, 1) ; 

} 

} 


// 

gluDeleteQuadric (glquad) ; 
glPopMatrix ( ) ; 

if  (big^cone  --  2) 

{ 

//  Draw  a  small  sphere  at  (ellipsoid,  cone)  kiss  point 
glPushMatrix  0 ; 

glTranslatef (xyzl_0[0] ,  xyxl_0[l],  xyzl_0[2]  );  //  location  of  vehicle  1 
glTranslatef (rl.kiss[0] ,  rl_kiss[l],  rl_kiss[2]  );  //  location  of  kiss  point 
for(ic«0;  ic<-3;  ic++)  glfColor[ic]  -  glfColors [4] [ic] ; 
glMaterialfv  (GL_FR0NT,  GL^AMBIENT.AND .DIFFUSE,  glfColor); 
glquad  -  gluNewQuadric () ; 

// 

gluSphere (glquad,  .2,  10,  10);  //  :  name,  rad  , slices,  stacks 

gluDeleteQuadric (glquad) ; 
glPopMatrix  0 ; 

} 

//  Draw  a  small  black  sphere  at  expected  intercept  point 
glPushMatrix 0 ; 

glTranslatef {xyz.T2_max[0] ,  xyz_T2.max [ 1 ] ,  xyz_T2.max [ 2 ] ) ;  //  location  of  intercept 
for(ic-Q;  ic<-3;  ic++)  glfColor[ic]  -  glfColors [7] [ic] ; 
glMaterialfv  (GL.FRONT,  GL.AMBIENT.AND.DIFFUSE,  glfColor); 
glquad  »  gluNewQuadric () ; 

// 

gluSphere (glquad ,  .2,  10,  10);  //  :  name,  :14i  , slices,  stacks 


•V  .\*  •N* 


gluDeleteQuadric (glquad) ; 
glPopMatrix ( ) ; 


/* - - - */ 

// 

//  Draw  the  intercept  surface  (aircraft  at  indentation) 

// 

intercept2  (xyzl_0,Vl_0,  xyz2_0rV2_0, 

£crl__error,T2_e_2,  x_s,y_s,z_s,  xyz_T2_max ,  accel_l,  accel_2); 
DrawSurface  (x_s,  y_s,  T2_e_2,  accel_l,  accel_2^  accel_l_limit ,  accel_2_limit ) 

/* - */ 

// 

//  Render  the  scene  in  the  pixel  buffer 
// 

SwapBuffers  (hdc); 

} 

/* - */ 

void  initialize_state (float  xyzl_0 [3] afloat  xy22_0 [3] afloat  V1_0 [3] afloat  V2_0[3], 

float  dx0[3],  float  Vi[3], float  V2[3]) 

{ 

int  i j ;  //  to  save  initial  aircraft, missile  state 

for(ij-0;  ij<-2;  ij++)  //  save  initial  aircraft , missile  state 

{ 

dxO[ij]  -  xyz2_0[ij]  •'  xyzl_0[ij]; 

Vl[ij]  *  V1^0[ij]; 

V2[ij]  -  V2J[ij]; 

} 

eta2_kl  -  cone2(Vl,  V2) ; 

} 

/* - */ 

float  cone2(float  Vl[3],  float  72(3]) 

//  Compute  eta2  such  that  vl/V2  *  sin (eta2)/eta2  *  a  +  b*eta2  +  c»eta2'^2  +  ... 

{ 

float  vl,  v2,  eta2_kl,  y,  w; 
float  a,b,c,d,e,f 
float  C,D,E,F,6; 

a  -  l.Of; 
b  -  -a/(2.f*3.f); 
c  -  -b/(4.f*5.f); 
d  «  -c/(6.f*7.f) ; 
e  -  -d/(8.f*9.f) ; 
f  -  -e/(10.f*ll.f) ; 
g  -  -f/(12.f*13*f); 

C  «  -c; 

D  -  2*c*c-b*d; 

E  *  -'5»c* (c*c-b*d)  -  b»b»e; 

F  -  7*c*c*(2*c*c-3*b*d)+3*b*b*(d»d+2*c*e)  -  b*b«b*f; 

G  -  42*c*c*c*(2*b*d-c*c)*’28*b*b*c* (d*d+c*e)+7*b*b*b* (d*e+c*f ) ~b*b*b*b*g; 

vl  -  (float)  sqrt(Vl[0]*Vl[0]+Vl[l]*Vl[l]+Vl[2]*Vl[2]); 
v2  -  (float)  sqrt(V2[0]*V2[0]+V2[l]*V2[l]+V2[2]*V2[2]); 
y  =  vl/v2; 
w  -  (y~a)/(b*b); 

/*  invert  the  series  for  y  -  sin(x)/x 
if  (vl<v2)  //  so  b*w  >  0 

eta2_kl  -  (float)  sqrt  (b*w*  ( 1+C*w+D*w*w+E*w«w*w  +  F*w*w*w*w  +  0*w*w*w*w*w ) ) ; 
return  eta2_kl; 

} 


/*  intercept2.c 

gcc  intercept2.c  -Wall  -Im 

This  program  plots  the  intercept  surface  for  a  missile  pursuing  an  aircraft. 
Mike  Elgersma  July  27,  1995  Mod  for  any  VI  Aug  20,  1995 
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For  vehicle_l  with  (x  in  and  initial  velocity  vector  VI (0),  the 

Z  locus  of  points  that  can  be  gotten  to  at  time  Tl,  given  any  possible 
CONSTANT  transverse  acceleration  is  given  by: 

I  I  X  -  xl(0)  M  -2  (  I  1  (X  -  xl(0))  X  V1(0)  I  I  ) 

Z  SI:  -  *  arctan( - )  -  Tl 

II  (X  -  xl(0))  X  V1(0)  II  (  (X  -  xl(0))  .  V1(0)  ) 

z 

z 

Z  For  vehicle_2  with  (x  in  R^3)  and  initial  velocity  vector  V2(0)^  the 
Z  locus  of  points  that  can  be  gotten  to  at  time  T2^  given  any  possible 
Z  CONSTANT  transverse  acceleration  is  given  by: 

X 

II  X  -  x2(0)  11-2  (  II  (3C  -  x2(0))  X  V2(0)  ||  ) 

Z  S2:  - *  arctan( - )  *  T2 

^  II  (X  ~  x2(0))  X  V2(0)  II  (  (X  -  x2(0))  .  V2(0)  ) 

Z  The  intersection  of  surfaces  SI  and  S2  gives  the  curve  on  which  the  tiro 
Z  vehicles  could  meet  at  time  Tl  -  T2. 

Z  The  union  of  all  such  curves  (union  over  all  intersect  times)  gives  a  surface 
Z  on  which  all  intercepts  must  occur. 

5/ - 

Z  Use  polar  coordinates  about  xl(0), 

^  [  V1_0  ]  [cos (eta 1)  ] 

Z  IL  -  xl(0)  *  rl(etal,zetal)»[ - ,  Vl_0_perp]* [sin (etal)*sin (zetal)  ] 

[||V1_0|1  ]  [sin(etal)»cos(zetal)] 

5: 

Z  rl  etal 

ZTl  - - 

::  V1(0)  sin(etal) 

Z  t2  eta2 

^  T2  -  — . . 

V2(0)  sin(eta2) 

Z  Set  rl_old  »  0 
Z  For  etal  -  pi-eps  to  0 
Z  For  zetal  «  -pi  to  pi 

^  rl2  -  I  x2(0)  -  xl(0)  I 
Z  For  iter  -  1  to  4 

Z  r2_over_rl2  «  sqrt  (rl_over__rl2*rl_over_rl2  -  2*cos_phi*rl_over_rl2  +1);  //law  of  cosines 

Z  eta2  -  acos(  (cos  los+rl  over  rl2*cos  gam)  /  r2_over_rl2  ) ;//  V2_unit  dot  x_rainus_x2_unit 

Z  k_of„eta  *  kO  ♦  eta2  /  sin(eta2); 

Z  rl_over_rl2  -  k_of_eta  •  r2_over_rl2; 

Z  End  iter 
5: 

Z  End  //  zetal 
Z  End  //  etal 

^ - 


rl2  -  I  x2(0)  -  xl(0)  I 

x2_minus_xl_unit  -  (x2(0)  -  xl(0))  /  rl2; 
v2  -  |V2(0)| 

V2_unit  -  V2(0)  /  v2; 
vl  .  |Vl(0)t 
Vl_unit  -  V1(0)  /  vl 
Vl_perp  - 

cos_los  =  -  x2_minus_xl_unit  dot  V2_unit 

For  etal  -  pi-eps  to  0 

cel  -  cos(etal);  sel  -  sin(etal); 
kO  -  (vl/v2)  »  sin (etal)/etal; 

For  zetal  -  -pi  to  pi 

czl  -  cos (zetal);  szl  "  sin (zetal); 

x_minus_x l_unit  -  [Vl__unit,  Vl_perp]* (cel,  il61*szl,  sel*czl) 


cos_phi  -  x2_minus_xl_unit  dot  x_iiiinus_xl_unit 
cos^gam  -  V2_unit  dot  x_niinus_xl_unit 

rl_over_rl2  -  0;  //  initialize  iteration 

//  Then  iterate  the  following  equations: 

For  ik  =  1  to  4 

r2_over_rl2  -  sqrt  (rl_over_rl2»rl_over_rl2  -  2*cos_phi*rl_over_rl2  +  1);  //law  of  cosines 
eta2  -  acos(  (cos_los+rl_over_rl2*cos_gaia)  /  r2_over_rl2  ) ;//  V2_unit  dot  x_minus_x2_unit 
k_of_eta  -  kO  *  eta2  /  sin(eta2); 
rl_over_rl2  -  k_of_eta  *  r2_over_rl2; 
end  //of  ik  loop 

//'Exit  iteration  with: 
rl  -  rl_over_rl2  *  rl2; 
r2  ■=  r2_over_rl2  *  rl2; 

T2  *  (r2/v2)  *  eta2/sin (eta2) ; 

x_surface  -  xl(0)  +  rl* (Vl_unit*cos_etal,  Vl_perp* (sel*szl,  sel*czl]); 
end  //  zetal 

end  //  etal  "  . . -  . 

5/ - 

*/ 

void  intercept2 (float  xyzl.0[3],  float  Vl_0[3],  float  xyz2_0[3],  float  V2_0[3], 
float  *rl_error,  float  T2_e_z[nuni_Z8ta+l]  [num_eta+l] , 
float  x_s[num_Z8ta+l] [num_eta+l],  float  y_s [num_zeta+l] [num^eta+l] , 
float  z_s[nuiii_zeta+l]  [nuni_eta+l]  ,  float  xyz_T2_max  [ 3 ]  , 
float  accel_l[num_zeta+l] [num_eta+l] , 
float  accel_2[num_zeta+l] [num_eta+l] ) 


{ 

int  i,  j,  k; 

float  rl,  rl_e_z[num_zeta+l] [num_eta+l] ; 
float  Vl_perp[3] [2] ,  cross[3],  norm; 
float  etal,  zetal,  eta2; 
float  Tl,  T2,  T2_max; 

- - 

float  rl2,  r2_over_rl2,  rl„ov0r_rl2,  rl_over_rl2_old; 

float  vl,  Vl_unit[3]; 

float  v2,  V2_unit[3]; 

float  cos_los,  cos_gain,  cos_phi ; 

float  sel,cel,  szl,czl; 

float  kO,  k_of_eta; 

float  eta2c;  //  convergence  test 

float  x_minus_xl_unit  [3] ,  x2_niinus_xl__unit  [3] ; 

rl2  -  (float)  sqrt(  (xyz2_0 [0] ~xyzl_0 [0] )  *  (xyz2_0 [0] -xyzl_0 [0] )  + 

(xyz2_0[l]-*xyzU[l])  *  (xyz2_0 [  1] -xyzl_Q  [  1] )  + 
(xyz2_0[2]-xyzl_0[2])  •  (xyz2_0  [2] -xyzlj [2] )  ); 
x2_i!iinus_xl_unit  [0]  -  (xyz2_0 [0] -xyzl_0  [0] )  /  rl2; 
x2_niinus_xl__unit  [1]  -  (xyz2_0  [l]-xyzl_0[  1] )  /  rl2; 
x2_minus_xl_unit [2]  -  (xyz2_0 [2] “xyzl_0[2] )  /  rl2; 

v2  -  (float)  sqrt  (V2_0[0]*V2_0[0]  +  V2_0 [  1  ] *V2_0 [  1  ]  +  V2_0 [2]*V2_0 [2] ) ; 

V2_unit[0]  -  V2_0[0]/v2; 

V2_unit[l]  -  V2_0[l]/v2; 

V2_unit[2]  -  V2_0[2]/v2; 

cos_los  -  -  x2_minus„xl_unit [0]*V2_unit [0] 

-  x2_minus__xl_unit [l]*V2_unit [1] 

-  x2_minus_xl_unit  [2]*V2_unit  [2] ; 

vl  -  (float)  sqrt  (V1_0[0]*V1_0[0]  +  V1_0[  1]*V1_0[  1]  +  V1_0  [2]*V1_0 [2] ) ; 

Vl_unit[0]  -  Vl_0[0]/vl; 

Vl_unit[l]  -  Vl_0[l]/vl; 

Vl_unit[2]  -  Vl_0[2]/vl; 

//  form  the  first  vector  orthogonal  to  V1_0 
if  (  fabs(Vl_0[0])  >  .5*vl  ) 

{ 

Vl_perp[0] [0]  -  -V1.0[l]/  (  (float)  sqrt (V1„0 [ 1]*V1_0 [ 1]  +  V1_0 [0]*V1_0[0] )  ); 
Vl_perp[l]  [0]  -  V1_0[0]/  (  (float)  sqrt (V1_0  17]*V1_0 [  1]  +  V1_0 [0]*V1_0 [0] )  ); 


O.Of ; 


Vl_perp[2][0]  - 

} 

else 

{ 

Vl_perp[0] [0]  -  O.Of; 

Vl„p0rp[l] [0]  -  Vl_0[2]/  (  (float)  sqrt (V1„0 [2]*V1_0 [2]  +  V1_0 [ 1]*V1_0 [ 1] )  ); 

Vl_perp[2] [0]  -  -V1_0[1]/  (  (float)  sqrt (V1_0 [2]*V1_0 [2]  +  V1_0 [ 1]*V1_0 [ 1] )  ); 

} 

//  form  the  second  vector  orthogonal  to  V1_0 
cross[0]  -  Vl_0[l]*Vl_perp[2] [0]  -  Vl_0[2]*Vl_perp[l] [0] ; 
cross[l]  -  Vl_O[2]*Vl_p0rp[O]  [0]  -  VU [0]*Vl_perp [2]  [0] ; 
cross[2]  «  Vl_0[0]*Vl_perp[l]  [0]  -  V1_0 [ l]*Vl_perp [0]  [0]  ; 

norm  *  (float)  sqrt (cross [0]*cross [0]+cross [ l]*cross [ i]+cross [2]»cross [2] ) ; 

Vl_perp [0] [ 1]  -  cross [0] /norm; 

Vl_perp [ 1] [ 1]  -  cross [ l]/noim; 

Vl_perp  [2]  [  1]  =*  cross  [2]/norm; 

T2_max  -  O.Of;  //  initialize 
*rl_error  -  O.Of;  //  initialize 

for  (j*0;  j<-nuin_eta;  j++) 

{  /*  start  eta  at  pi,  where  rl-0  is  the  correct  answer  */ 
etal  *  (pi  *  (num^eta-j )  )/nuin,_eta; 
sel  *  (float)  sin(etal); 
cel  »  (float)  cos (etal); 

if  (  fabs(etal)  <  .0001)  kO  *  vl/v2;  //  sin(etal)/etal  «  1 

else  kO  -  (vl/v2)  •  (float)  sin (etal)/etal; 

for  (i»0;  i<*num_zeta;  i++) 

{ 

zetal  -  (2*pi  *  i  )/num__zeta; 
szl  *  (float)  sin(zetal); 
czl  -  (float)  cos (zetal); 

x_minus_xl_unit [0]  -  Vl_unit [0]*cel  +  Vl_perp [0] [0]*sel*szl  +  Vl_perp[0] [ l]*sel*czl; 
x_minus_xl_unit [1]  -  Vl_unit [ l]*cel  +  Vl_perp [ 1] [Q]*sel*szl  +  Vl_perp[ 1] [l]*S0l*czl; 
x_minus_xl_unit [2]  *  Vl_unit [2]*cel  +  Vl_perp [2] [0]*sel*szl  +  Vl_perp[2] [l]*sel»czl; 

cos_phi-  x2_minus_xl_unit [0]  *  x_niinus_xl_unit [0]  + 
x2_minus_xl_unit [ 1]  *  x_minus_xl_unit [1 j  + 
x2_minus_xl_unit [2]  *  x_minus_xl_unit [2] ; 
cos._ gam  -  V2_unit[0]  •  x_minus_xl_unit [0]  + 

V2_unit[l]  *  x_minus_xl_unit [ 1]  + 

V2_unit[2]  *  xjminus_xl_unit [2] ; 

if  (j<2) 

{ 

rl  =  O.Of; 

rl_over_rl2  -  rl  /  rl2; 

} 

else 

{ 

rl  =  rl_e_z[i] [j-1] ;  /*  last  value  (comprimise  speed  vs  diverge)  */ 

rl  *  O.Of;  //  prevent  divergence 

rl  *  2*rl_e_z[i ] [j-1]  -  rl_e_z [i ] [j -2] ;  /*  linear  interp  (speedier)  */ 
rl_over_rl2  -  rl  /  rl2; 

} 

for  (k-1;  k<-nuin_iter;  k++) 

{ 

rl_over__rl2_old  -  rl_over_rl2;  //  save  old  value  for  convergence  test 

r2_over_rl2-(float)sqrt (rl_over_rl2*rl_over_rl2-2*cos_phi*rl_over_rl2+  1) ;//law  of  cosines 
eta2- (float )acos ( (cos_los+rl_over_rl2*cos_gam)/r2_over_r  12)  ;//V2_unit  dot  x_minus_x2_unit 
k__of_eta  -  kO  *  eta2  /  (float)  sin(eta2); 
rl_over_rl2  *  k_of_eta  *  r2_over_rl2; 

}  /*  end  of  k  loop  */ 

T1  -  (rl_over_rl2_old*rl2/vl)  *  etal/  (f loat )sin (etal) ; 

T2  -  (r2_over_rl2  *rl2/v2)  *  eta2/  (float )sin (eta2) ; 

eta2c  -  (float)  (3.14159  -  asin(vl/v2)  +  acos (cos_los)  )  /  2; 

//if  (  (fabs(Tl  -  T2)  <  .1)  &  (eta2  <  eta2c)  )  //  convergence  test 

if  (  fabs(Tl  -  T2)  <  .00  )  //  convergence  'Ifet 


{ 

rl  -  rl_over_rl2  •  rl2; 

T2_e_z[i][j]  -  T2; 

} 

else 

{ 

n  “  rl_e_z[i] [j-l];  //  no  convergence,  so  use  last  value  of  radius 

T2_e_z[i][j]  =  O.Of;  //  no  convergence  flagged.  Later,  set  color  -  transparent 

rl_e_z[i ] [j ]  -  rl; 
x_s[i][j]  -  zyzl_0[0]  + 

rl*(Vl_unit[0]*cel  +  Vl_perp [0] [0]*sel*szl  +  Vl_perp [0] [ l]*sel»czl) ; 
y-s[i][j]  ■  xyzl_0[l]  + 

rl«(Vl_unit[l}»cel  +  Vl_perp [1] [0]»sel*szl  +  Vl_perp[l] [l]*sel*czl) ; 

Z-S[i][j]  -  xyzl_0[2]  + 

rl*(Vl_unit[2]«cel  +  Vl_parp[2] [0]*sel*szl  +  Vl_perp[2] [l]*sal»czl) ; 

accel_l[i] [j]  -  2. f«etal  *  vl/Tl;  //  angle»radius  -  v«T,  accel  -  v*v/radius 
accel_2[i ] [j ]  «  2.f»eta2  •  v2/T2; 

if(T2_max  <  T2) 

{ 

T2_max  -  T2; 

xyz_T2_max[0]  -  x_s[i][j]; 
xyz_T2_max [ 1 ]  -  y_s[i][j]; 
xyz_T2_max [2]  -  2_s [i ] [ j ] ; 

} 

•rl_erTor  <  fabs(rl_over_rl2-rl_over_rl2_old)*rl2)  //  0  if  converged 

{  *rl_error  -  (float)fabs(rl_over_rl2-rl_over_rl2_old)*rl2;  } 

}  end  of  i  loop  •/ 

}  /•  end  of  j  loop  •/ 

}  /•  end  of  function  intercept 2  •/ 


/• 


void  cross_prod( float  cross[3],  float  vecl[3],  float  vec2[3]) 

{ 

cross[0]  -  vecl[l]*vec2[2]  -  vecl[2]*vec2[l] ; 
crossfl]  -  vecl[2i»vec2[0]  -  vecl[0]«vec2t2] ; 
cross[2]  -  vecl[0i*vec2[l]  -,vecl[l]*vec2[0] ; 

} 
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clear 

h  =  figure('PaperPo3ition', [0.5,0.5,8,10]) ;  %  So  "print"  gives  large  figure 

dataset  *=  13; 
if  <dataset==0) 


%  If  [u,v,w]* [1,1,1]  is  a  solution,  then  a+b*=sqrt 
%  a  =  rand{l,l);  b  *  sqrt(3)-a; 

%  de  -  rand(2,l);  def-[de;sqrt  (3)  -  [1, 1]  *de] 

%  k  -  1; 


(3)=d+e+f  and  k*k-=l 

%  Cone_l  coeff 
%  Cone_2  coeff 
%  Ellipsoid  coeff 


elseif  (dataset“l) 

%  Get  all  8  solutions  REAL  for  the  following  data. 

%  Only  2  solutions  satisfy  the  HALFcone  restriction  to  within  I|err||  <  .06 

%  Must  comment  out  the  reconputation  of  etal  and  eta2  inside  the  ii, jj  loops. 

ab0_0  *  [6;  14;  0];  %  Cone_l  coeff.  If  a*b«0,  then  poly (u"^2, v'^2, w^2) 

cief_0  -=  [3;  4;  7];  %  Cone_2  coeff 

kO  *=  1.5;  %  Ellipsoid  coeff 

rl_  -  [  1;0;0] ; 

r2_  -  [-1;0;0] ; 

etal  “  acos  (l/norm{ab0_0) )  ; 

eta2  «  acos  (l/norTti{def_0‘) )  ; 

Vl_  -  ab0_0;  %  times  any  scalar 

V2_  «  (def_0/norm(def_0) )  *  (norm{Vl_)  *eta2*sin  (etal) )  /  (  k0*etal*sin  (eta2) ) 

%_r - 

elseif  (dat aset"2 ) 

%  Two  eta  half-cones  intercept  in  2  -circles 
rl_  -  [  1;0;0] ; 
r2_  -  [-1;0;0]; 

Vl_  -  [-1;  .1;  .3  ]; 

V2_  -  [  3;  .2;  .1  ] ; 

% - 

%  Note  that  datasets  3, 4, 5, 6  have  Vl_,  V2_,  and  r2_  -  rl_  nearly  orthogonal 
elseif  (dataset==3) 

%  Solution— Sphere  just  smoothly  merges  with  Solution-half-cone 

rl_  =  [0;0;0]; 

r2_  =  [.10;  3.;  -2  ] ; 

Vl_  -  [1.15;  .1;  .1  ];  %  |V2|/|V1|  -  3.113  <  pi 

V2_  =  [.10;  2.;  3.] ; 

% - 

elseif  (dataset==4) 

%  Solution-Sphere  just  touches  Solution-half-cone 

rl_  “  [0;0;0] ; 

r2_  -  [.10;  3.;  -2  ] ; 

Vl_  -  [1.147;  .1;  .1  ];  %  |V2|/|V1|  •=  3.121  <  pi 

V2_  -  [.10;  2.;  3.] ; 

% - 


elseif  (dataset““5) 

%  Solution-Sphere  just  misses  Solution-half-cone 

rl_  •=  [0;  0;  0] ; 

r2_  -  [.10;  3.;  -2  ] ; 

Vl_  =  [1.144;  .1;  .1  ];  %  |V2|/|V1|  =  3.129  <  pi 

V2_  -  [.10;  2.;  3.]; 

% - 

elseif  (dataset“6) 

%  Solution-Sphere  just  misses  Solution-half-cone 

rl_  -  [0;0;0] ; 

r2  =  [.10;  3.;  -2  ]; 
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Vl_  “  [1.14;  .1;  .1  ];  %  |V2|/|V1|  -  3.140  <  pi 

V2_  -  [.10;  2.;  3.] ; 

% - 

elseif  {dataset*=7) 

%  Get  4  real  and  4  complex  solutions. 

%  The  4  real  solutions  satisfy  the  HALFcone  restriction. 

%  Must  comment  out  the  recomputation  of  etal  and  eta2  inside  the  ii,jj  loops. 

ab0_0  *=  [~.6;  1.0;  .0  ];  %  Cone_l  coeff.  If  a*h=0f  then  poly (u'^2, v^2, w'"2) 

def_0  «  [-.3;  1.3;  .001];  %  Cone_2  coeff 

kO  *  1.02;  %  Ellipsoid  coeff 

rl_  -  [  1;0;0]; 

r2_  -  [-1;0;0] ; 

etal  *  acos  (l/norm(ab0_0) )  ; 

eta2  “  acos {l/norm(def_0) ) ; 

Vl_  «  ab0_0;  %  times  any  scalar 

V2_  -  (def_0/norm{def_0) )  *  (norm(Vl_)  *eta2*sin  (etal) )  /  (  k0*etal*sin  (eta2) ) 


elseif  (dataset-“8) 

%  Random  data  in  5-dimensional  set 
rl_  «  [  1;0;0]; 
r2_  «=  [-1;0;0]; 

Vl_  -  [rand(2,l) ;  0]  ; 

V2_  -  rand(3, 1) ; 

% - 

elseif  (dataset“9) 

%  Yearly  Report  values  used  in  figures  1  to  8; 
rl_  -  [0;0;0]; 
r2_  -  [6;  -5;  4] ; 

Vl_  «  [-.3;  -.1;  -.1]; 

V2_  «  [-.2;  0;  -.6]; 

% - 

elseif  (dataset=“*10) 

%  Try  to  find  cases  where  •’sphere”  and  "cone”  touch 
rl_  -  [  1;0;0]; 
r2_  «  [-1;0;0]; 

tmp  -  rand{3, 1)  ; 

Vl_  «=  [rand(2,l);  0]; 

“  pi*norm(Vl_)  *tir5>/norm(tii^)  ;  %  so  v2/vl  «  pi 


elseif  {dataset“ll) 

%  Put  slow  airplane  almost  behind  fast  missile 
rl_  =  [  1;0;0];  %  airplane 
r2_  «  [-1;0;0];  %  missile 
Vl_  *=  [rand(2,l);  0]  ; 

V2_  -  [-3;  .1;  .2] ; 

% - 

elseif  (dataset==12) 

%  Degenerate  back-substitution 
*  [  1;0;0];  %  airplane 
r2_  «  [-1;0;0];  %  missile 
Vl_  -  [  0;  1;  0]; 

V2_  -  [  0;  0;  1] ; 

% - 

elseif  (dataset*==13) 

%  Solution  set  conponents  just  touch  when: 

%  (norm (V2_) /norm (Vl_) )  *  (sin  (eta_c) /eta_c)  *  sin(eta_c  -  los)  *  1 

%  and  etal  ■*  1  at  that  point. 

r2__  *  rand  (3,1);%  missile 

rl_  *  rand(3,l);%  airplane 

v2  *  rand;  junk  «  rand(2,l);  los  «  1.4 

V2_  «  v2*[(rl_  -  r2_)/norm(rl_  -  r2_) ,  null((rl_  -  r2_) ' )  ]  *  ... 
[cos (los) ;  sin (los) * junk/norm( junk) ] ; 

los  *=  acos  (  V2_'*(rl_  -  r2_)  /  (norm(V2_)  *norm(rl_  -  r2  ) )  ) 
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%  max  [sin (eta)  ] 

%  V1/V2  <  [ - *  sin  (eta  -  los)  ] 

%  0<los<eta<pi  [  eta  ] 

%  (los)  max  [sin(eta) 

%.724*  (.99-sin( - ))  <  [ - *sin(eta 

%  (  2  )  0<los<eta<pi[  eta 


for  separate  solution 

components 

]  los 

los) ]<.724* (1. 03-sin ( - ) ) 

]  2 


vl_over__v2»= .  724*  (0 . 99-sin  (los/2) ); %merged  solution  component  for  some  unit_Vl 
vl_over_v2«. 724*  (1 .30-sin  (los/2) ) ;  %  separate  solution  cortponents 
vl_over_v2*. 724* (1 . 01-sin (los/2) ) ;  % 

Vl_dir  -  skew(r2_  -  rl_)  *skew (r2_  -  rl_)  *V2_;  %  perp  to  rl2  in  [rl2,V2]  plane 
Vl_dir  «  Vl_dir  +  . 001*rand(3^ 1) ;  %  planar  data  (degenerate  order8_poly) 
unit_Vl  *=  Vl_dir/norm(Vl_dir)  ; 

Vl_  *  vl_over__v2*norm(V2_)  *unit_Vl; 

% - 

end 


los  «  acos  (V2_'*  (rl__-r2_)  /  (norm(V2_)  *norm(rl_-r2_) )  )  ; 

%  sin(2*eta2c  -  los)  *  vl_over_v2  at  the  max 

eta2c  “  (pi  -  asin (vl_over_v2)  +  los)/2;  %  angle  from  V2_  to  touch  point 
ration (norm (V2  ) /norm (Vl_) )* (sin (eta2c) /eta2c) *sin (eta2c-los) %  >  1  separate  sol 


unit  “  (^1_  ”  r2_) /norm(r2_  -  rl__)  ; 

Rot  *  [unit,  null (unit' ) ] ; 

Rot  *=  Rot*diag([l;  1;  det(Rot)]); 

stretch  »  2/norm(rl_  -  r2_) ; 
orig  “  (3:2_  +  rl__)/2/ 
rl_s  *  stretch*Rot' * (rl_  -  orig); 
r2_s  ■=  stretch*Rot' *  (r2_  -  orig); 

Vl_s  *  stretch*Rot' *V1_; 

V2_s  «  stretch*Rot'*V2_; 

ca  -  Vl_s(2)/norm(Vl_s(2:3))  ; 
sa  «  Vl_s  (3) /norm(Vl_s  (2  :3) )  ; 

Rotx  =  [1,  0,  0; 

0,  ca,  sa; 

0,-sa,  ca] ; 

VI  =  Rotx*Vl_s;  %  ■=  [xxx,yyy,  0] 

V2  «  Rotx*V2_s; 

% - 

eps  =  .0013; 

ii_max  *  400 

jj_max  *  100; 

n_pts  ~  ii_jnax*  j  j_max; 

sol_set  *  zeros (3, 8*n_pts) ; 

max^sols  *=  0;  %  max  number  of  real  solutions  that  satisfy  HALF-cone 

num^^rootjmultl  “  0; 

num_root_mult2  “0; 

nxam_root__mult4  -  0; 

num_0_sols  -  0; 

num_l_sols  «  0; 

num_2_sols  “0; 

num_3_sols  -  0; 

num_4_sols  =  0; 

num_5_sols  *  0 ; 

num_€_sols  *  0; 

num_7_sols  =  0; 

num  8  sols  **  0; 


for  ii“0 : ii_max-l; 
ii 

for  j j«0 : j j_max-l; 
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%  eta2  in  outer  loop^  since  eta2<eta2c  *>  solution  component  1 
eta2  «  (pi) *ii/ii_max  +  eps;  %  eps  avoids  3in(0)/0 
etal  =  (pi)  *  j  j/ j  j_jnax  +  eps;  % 

abO  -  VI/ (norm(Vl)  *co3  (etal)  )  ;  a  »=  abO(l);  b  =  abO  (2)  ; 

def  -  V2/(norm(V2)*cos(eta2));  d  *  def(l);  e  «  def(2);  f  *  def(3)/ 

k  »  (norm (VI) *eta2*sin (etal) ) /  ... 

(norm(V2) *etal*sin (eta2) ) ; 
mil  -=  d*d-l;  m22  «  e*e-l;  m33  =  f*f-l; 

ml2  «  d*e;  in23  -  e*f;  ml3  “  f*d; 

Ellipsoid  «  diag(  [-l,k*k,k*k«-l,k*k-’l] )  ; 

% - ; - 

%  All  polynomials  are  wrt  to  variable  v. 

v2  -  [1,  0,  0];  v4  «  [1,  0,  0,  0,  0] ; 

v6  *  [1^  0,  0,  0,  0^  0/  0];  u0v8w0  ■=  [1,  0,  0^  0,  0^  0^  0,  0,  0]; 

u2  -  (l/(k*k))*[(l-k*k)*b*b,  (l-'k*k)  *2*a*b,  (l-k*k)  *  (a*a-l)  +  1]  ; 

u4  *  conv.(u2/:_u2).i-  uSvOwO  ««  conv(u4^  u4)  ; 

w2  -  [b*b~l,  2*a*b,  a*a-l] ; 

w4  =  conv(w2,  w2) ;  w6  =  conv(w4,  w2) ;  uOvOwS  =  conv(w4,  w4) ; 

u6v2w0  -  [u6/  0,  0];  u4v4w0  *  [u4,  0,  0,  0,  0];  u2v6w0  ■=  [u2, 0, 0, 0, 0, 0, 0]  ; 

u6v0w2  “  conv(u6/w2);  u4v2w2  [conv (u4,  w2)  ,  0,  0]  ;  u2v4w2  =  [conv  (u2,  w2) ,  0,  0,  0, 0] 

u0v6w2  *  tw2,  0,  0,  0,  0,  0,  0]  ;  u4v0w4  =  conv(u4,w4);  u2v2w4  [conv (u2/ w4) ,  0,  0]  ; 
u0v4w4  *  [w4, 0, 0, 0, 0] ;  u2v0w6  =  conv(u2,w6);  u0v2w6  *=  [w6,0,0]; 


order8_jpoly  *=  ... 

mll'^4  *  u8v0w0  +  . .  . 
m22''4  *  u0v8w0  +  . .  . 
in33''4  *  u0v0w8  +  .  .  . 
2*  (8*ml2'^2*  (ml2'^2  -  mll*in22)  +  3*mll'"2*m22'"2)  *  u4v4w0  +  ... 

2*  (8*ml3'"2*  (ml3"2  -  mll*m33)  +  3*mll^2*m33"^2)  *  u4v0w4  +  ... 

2*  (8*m23"^2*  (m23'"2  -  m22*m33)  +  3*m22^2*m33^2)  *  u0v4w4  +  ... 

4*mll^2*  (mll*m22  -  2*ml2'"2)  *  u6v2w0  +  ... 

4*mll^2*  (mll*m33  -  2*ml3'^2)  *  u6v0w2  +  ... 

4*m22"2*  (mll*m22  -  2*ml2'"2)  *  u2v6w0  +  ... 

4*in22^2*  (m22*m33  -  2*m23'^2)  *  u0v6w2  +  ... 

4*m33'^2*  (m22*m33  -  2*m23"2)  *  u0v2w6  +  ... 

4*m33'"2*  (mll*m33  -  2*ml3^2)  *  u2v0w6  +  ... 

4*(  mll^2*  (3*m22*m33  »  2*m23^2)  -  4*mll*  (m22*ml3"2  +  m33*ml2'"2)  +  ... 

8*ml2*ml3*  (2*mll*m23  -  ml2*ml3)  )  *  u4v2w2  +  ... 
4*(  m22"2*  {3*ni33*mll  -  2*ml3'^2)  -  4*in22*  (mll*m23"2  +  m33*ml2^2)  +  ... 

8*ml2*in23*  (2*m22*ml3  -  ml2*rti23)  )  *  u2v4w2  +  ... 
4*(  m33'^2*(3*mll*m22  -  2*ml2''2)  -  4*m33*  (mll*m23''2  +  m22*ml3''2)  +  ... 

8*ml3*m23*  (2*m33*ml2  -  ml3*m23)  )  *  u2v2w4; 

order8_poly  -  order8_poly  /  sqrt  (order8_poly  (1)  *  order8_poly  (9)  )  ; 

%  Linear  poly  in  uv,  whose  coeff  are  poly  in  u2,v2,w2: 

uv0_coeff  -  mll'^2*u4  +  (4*ml2"2  +  2*mll*ni22)  *conv  (u2,  v2)  +  m22"2*v4  +  ... 

(-4*ml3'^2+2*mll*m33)  *conv  (u2,  w2)  +  (-4*m23'^2+2*m22*m33)  *conv  (v2,  w2)  +  m33'^2*w4 
uvl_coeff  «  4*  (mll*ml2*u2  +  ml2*m22*v2  -  2*ml3*m23*w2  +  ml2*m33*w2)  ; 

V  *  zeros (8,1);  u  *  zeros (8,1);  w  «  zeros (8,1); 

X  =  zeros  (8,1);  y  »  zeros  (8,1);  z  *=  zeros  (8,1); 

% - 

%  If  the  data  is  planar  the  8th  order  poly  factors. 

%  If  8th  order  polynomial  is  the  square  of  a  4th  order  poly,  "roots"  may  crash 
% 
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[order4_poly/  err_p4]  »  factor_poly (order8_poly) ; 
if  (err_p4  <  .005) 

[order2_poly,  err_p2]  -  f actorjpoly  (order4jpoly)  ; 
if  (err_p2  <  .005) 

root_multiplicity  4; 
v_quad  *  roots (order2_poly) ; 
for  iv*l:2; 

v(4*iv-3)  «  v_quad(iv); 
v(4*iv-2)  *=  v_quad(iv); 
v(4*iv-l)  »  v_quad(iv); 
v(4*iv-0)  -  v__quad(iv); 
end 
else 

root_multiplicity  «  2; 
v^double  *  roots {order4_poly) ; 
for  iv»l:4; 

v(2*iv-l)  -  v_double (iv) ; 
v(2*iv-0)  -  v_jdoiable  (iv) ; 
end 
end 
else 

root_multiplicity  -1; 

V  -  roots (orderB^poly) ; 

end 

% - 

if  (  root_multiplicity  <“  4) 
for  j  -  1:8 

u(j)  -  -polyval(uv0_coeff,v(j) )  /  (  v(j)  *  polyval (uvl_coef f , v( j) )  ); 

end 
else 

for  j  »  1:4 

u{2*j“l)  -  sqrt(  polyval  (u2,v (2*  j) )  ); 
u(2*j  )  -  -sqrt(  polyval (u2, v (2* j) )  ); 

end 
end 

if  (  root_multiplicity  “  1) 
for  j  *«  1:8 

%  Linear  poly  in  w,  whose  coeff  are  poly  in  u,v^w2 

w0_coeff  -  m33*polyval(w2,v(j) )  +  mll*u(j)^2  +  2*ml2*u ( j) *v ( j)  +  in22*v(j)"2; 
wl_coeff  -  2*(ml3*u(j)  +  m23*v(j));  %  wl_coeff=0  (since  f=0)  for  planar  data 
w{j)  =  -w0_coef f /wl_coeff ; 
end 

elself  {  root_multiplicity  —  2) 
for  j  “  1:4 

w(2*j-l)  -  sqrt (  polyval (w2,v (2* j) )  ); 

w(2*j-0)  •=  -sqrt  {  polyval  (w2,v(2*j) )  ); 

end 

elseif  (  root_multiplicity  •*=  4) 
for  j  "  1:2 

w{4*j-3)  “  sqrt(  polyval (w2, v (4* j) )  ); 
w(4*j-2)  -  sqrt{  polyval (w2,v(4*j) )  ); 
w(4*j-l)  -  -3qrt(  polyval (w2,v( 4* j) )  ); 
w(4*j-0)  -  -sqrt {  polyval (w2,v( 4* j) )  ); 
end 
end 

for  j  —  1:8 

%  Bi-Rational  transformation 
x(j)  -  (u{j)+l)/(u(j)-l); 
y(j)  -  2*v( j) / (u( j) -1) ; 

z{j)  -  2*w(j)/{u{j)-l); 

end 

uvw  =  [u,v,w]; 
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xyz  -  [x,y, z] ; 

n__sols  *  0;  %  count  how  many  of  the  8  roots  are  real  and  satisfy  HALF-cone 
eps_err  *=  .05; 
for  j=l:8 

vec  -  [x(j)-l;  x(j)+l;  y(j);  z(j)]; 
vec_l  -  [x(j)-l;  y(j);  z(j)]; 
vec_2  -  [x{j)+l/  y(j);  z(j)]; 

err4  ■=  norm{vec_l)  -  k*norm(vec_2) ;  %  Ellipsoid  is  square  of  this. 

err5  =  Vl'*vec_l  -  cos  (etal)  *norm{Vl)  *norm(vec_l) ;  %  HALF  cone__l 

err6  «  V2'*vec_2  -  cos  (eta2)  *norm(V2)  *norm(vec_2) ;  %  HALF  cone_2 

%disp  ( [  sprintf  (' err4,  errS,  err6  *=  %g  %g  %g' ,  err4,  err5,  err6)  ]) 

if  (  (  abs  (iinag{xyz  ( 1) ) )  <  .02  +  .  02*abs  (real  (xyz  ( j,  1) ) )  )  &  ... 

(  abs  (iinag(xyz(  j/2)  )  )  <  .02  +  .  02*abs  (real  (xyz  ( 2) ))  )  &  .  .  . 

(  abs  (iinag(xyz  ( j,  3) ) )  <  ,02  +  .  02*abs  (real  (xyz  ( j,  3) ) )  )  &  ... 

(norm( [err4,err5, err6] )<eps_err)  ) 
sol_set(:,  j  +  8*  (ii*  j  jjnaax  +  jj)  )  »=  xyz(j,:)'; 
n_sols  *=  n_sols  +  1; 

if  (rootjmultipiicity:.  :  »~1)  num_root_multl  «  num^rootjmultl  +  1;  end; 

if  (root_multiplicity  ***2)  nuiti_root_mult2  *  num_root_jnult2  +  1;  end; 

if  (root_multiplicity  “4)  num_root_inult4  -  num_root_mult4  +  1;  end; 

else 

sol_set(:,  j  +  8*  (ii* j  j_inax  +  jj)  )  *  [0;0;0]; 
end 


end 

if (n_sols 

0) 

num_0_ 

sols 

1= 

num_ 

0 

^sols 

+ 

1 

end; 

if  (n__sols 

1) 

num_l_ 

sols 

« 

num_ 

‘r 

sols 

+ 

1 

end; 

if (n_sols 

2) 

num_2_ 

sols 

mt 

num_ 

“2“ 

^sols 

+ 

1 

end; 

if (n_sols 

3) 

num_3_ 

sols 

num_ 

V 

sols 

+ 

1 

end; 

if (n_sol3 

4) 

num_4_ 

sols 

« 

n\im_ 

'4“ 

sols 

+ 

1 

end; 

if (n_sols 

5) 

num_5_ 

sols 

num_ 

v 

sols 

+ 

1 

end; 

if (n_sols 

6) 

num_6_ 

sols 

ss 

num_^ 

“6“ 

sols 

+ 

1 

end; 

if (n_sols 

7) 

num__7_ 

sols 

num_ 

“7“ 

sols 

+ 

1 

end; 

if (n_sols 

8) 

num__8_ 

sols 

* 

num_ 

"8^ 

sols 

+ 

1 

end; 

if  (n_sols 

> 

max  sols) 

max  sols 

n 

sols 

etal_max_sols  *=  etal 
eta2_max__sols  eta2 

multiplicity_sol3  *  root_multiplicity 
end 

end  %  end  of  jj  loop 
end  %  end  of  ii  loop 

max__sols 

%  rotate  and  project 

aa  =  -.37;  bb-  .34;  cc  *=  .35;  %  three  small  angles  (radians) 
aa  =  .47;  bb=-.44;  cc  =  -.45;  %  three  small  angles  (radians) 

S  **  [  0  cc  -bb; 

-cc  0  aa; 

bb  -aa  0] ;  %  skew  symmetric 

C  *  (eye  (3) -S)  /  (eye  (3) +S)  ;  %  Cayley  Transform  from  R''3  to  SO  (3) 

sol_set_rot  =  C*sol_set; 

soll  “  8* j j_inax*ii_inax*eta2c/pi;  %  solution  coitponent  1 
3012  8* j jjmax*ii_max;  %  solution  coxrponent  1 

plot (sol_set_rot (1, 1 : soil) , sol_set_rot (2, 1 : soli) ,  ' . ' ) ; 
hold  on 

plot (sol_set_rot (1, 1+soll ; sol2) , sol_set_rot {2, 1+soll :sol2) ,  ' +' ) ; 
dr  *=  r2_  -  rl_; 

title_txtl=sprintf {'x2-xl=(%5.3f  %5.3f  %5.3f  )/  dr(l),dr(2),dr(3)); 

title_txt2=sprintf ('Vl«(%5.3f  %5.3f  %5.3f  ),  ^  Vl_(l) ,  Vl_(2) ,  Vl_(3)); 

title_txt3*=sprintf  ('V2*(%5.3f  %5.3f  %5.3f  V2_(l),  V2_(2),  V2_(3)); 

title ([title  txtl,  title_txt2,  title_txt3] ) 


f- 


i: 


f 

1:; 
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delta^r  -  norm(rl_s  -  r2_s)  *  tan  (  eta2c  -  los) ; 

kiss _point  -  C*  {  rl_s  +  delta_r*Vl/norm(Vl)  );  %  in  screen  coordinates 
plot (  kissjpoint (1) /  kiss_point (2) ,  'o') 

airplane_tail  -  C*  (rl_s  -  VI/. 2)  ; 
airplane^nose  *  C*rl_s; 

plot(  airplane_nose (1) /  airplane_nose (2) ,  'o'); 

plot  ( [airplane^nose  (1)  /  airplane _ ^tail  (1)  ]  #  [airplane_nose  (2) , axrplane^tail  (2)  ] ) ; 

missile_tail  *  C* (r2_s  -  V2/.2); 
missile^nose  *  C*r2_s; 

plot  (  raissile_nose  (1) ,  missile^nose  (2) ,  '*'); 

plot  ( [iaissile__nose  (1)  ,missile_tail  (1)  ]  #  [missile_nose  (2)  ,missile_tail  (2)  ] )  ; 

%  The  origin  is  loidway  between  the  airplane,  rl_s**  (1,  0, 0)  and  missile  (-*1,  0, 0) 
axis  ([-1,2, -2,1]); 

%axis ([-3, 3, -3, 3] ) ; 

%axis ( [-5, 5, -5, 5] ) ; 

%axis([-10,10,-10,10] ) ; 
hold  off 

xlabel {' half cone (etal)  half cone (eta2)  ellipsoid (etal,eta2)  0<etal<pi  0<eta2<pi' ) 
v2_over_vl  *  norm(V2__) /norm(Vl_)  ; 
v2_over__vl_touch  »=  1/  (  .724*  (1 . 01-sin  (los/2) )  ); 

yt«sprintf  ('v2/vl  «  %3.2f,  v2/vl_touch  =  %3 .2f ' ,  v2_over_vl,  v2_pver_vl_touch)  ; 
ytext2  “  sprintf('  los  =  %1.2f,  max^sols  *  %1.0f',  los,  max^^sols)  ; 
ytext3  *  sprintf('  eta2c  ■■  %1.3f  ',  eta2c)  ;  , 

ytext4*sprintf  ('kissjpt  -  xl  +  %5.3f  *  Vl_unit ', delta_r /stretch)  ; 
ylabel([yt,  ytext2,  ytext3,  ytext4] ) 

%  print 

num_124_root_mult  =  [nxim_root_multl  num_root_mult2  num_root_jnult4] 

real_012345678_sols  «  [num_0_sols  nvim_l_sols  nLiin_2__sols  num_3_sols  . . . 

num_4_sols  num_5_sols  nuin_6_sols  num_7_sols  n\am_8_sols] 
disp { ' num___124 _ root_mult  *  [1;1;1]  *  real__^012345678 _ sols  *  [0;1;2;3;4;5;6;7;8]') 


save  intercept_pol_dat 
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%  vlv2_vs_los  .m  Mike  Elgersma  Deceinber  22,  1995 

% - 

%  See  "Components  of  the  Intercept  Surface"  in  ecalm/nonlin/yearly_report .Oct95 
%  See  "Components  of  the  Intercept  Surface"  in  ecalm/nonlin/yearly_report .Dec95 
% 

%  Cone  and  ellipsoid  just  touch  (double  root)  when  vl/v2  ••  f(eta,  los) 

% 

%  For  each  value  of  los, 

%  1)  confute  the  max,  wrt  eta,  of  f  (eta, los) 

%  2)  set  max  vl/v2  *=  max  f 

%  Plot  max  vl/v2  vs  each  such  los. 

% - 

%  Find  smallest  vl/v2  that  causes  accel_2  to  hit  some  constraint : 

% 

%  2*v2*v2*sin (eta2) 

%  accel__2  * -  solve  for  r2  and  plug  into  next  equation: 

%  r2 

%  Intercept  surface  just  touches  constant  accel_2  surface  at: 

% 

%  sin(etal)  VI  (  rl  sin(eta2))  (  | 1 (r_l«r_2) -r2*l_r2 | |  sin(eta2)} 

% - -  «.  (  — - )  =  min  {  - - * — “} 

%  etal  V2  eta2  {  r2  eta2  )  {  r2  eta2  ) 

% 

%  (11  (cos (los))  (cos(eta2))  ||  ) 

%  (11  rl2* (sin (los) )  -  r2* (sin (eta2) )  ||  sin(eta2)  } 

%  -  min  (  - - - —  - — - ) 

%  {  r2  eta2  ) 

% 

%  and  rl2/r2  «  accel_2  /  accel_12*sin (eta2)  where  accel_12  *  2*v2*v2/rl2 
% 


%  sin (etal) 

VI 

1  lacel  2 

(1) 

(cos (eta2-los) 

1 1 

% - - 

— 

*  min 

{ 

1 1 - 

(0) 

-  sin (eta2) * (sin (eta2-los) ) 

1 1  /  eta2 

%  etal 

V2 

eta2 

{ 

1 1 accel_12 

1  1 

% 

%  <  {  accel_2/accel_12  -  sin (los)  )  /  los  (set  eta2  «  los) 

% 

% - 

h*  f igure ('PaperPosition' , [0 .5, 0 .5, 8, 10] ) ; .  %  So  "print"  gives  large  figure 

epss  =  le-9; 
num_los  *=  100; 

los_min  =  epss;  los^max  =  pi-epss;  %  0  <  los  <  pi 


los  *  zeros 

max_vl_ove  r_v2  »  zeros 

lowe rebound  zeros 

middle  **  zeros 

difference  *=  zeros 

upper_bound  *=  zeros 

eta__at_max  **  zeros 

eta_at_max_  -  zeros 

V  l__o  ve  r_v2_i  t  e  r_con 
d  los  “  (los  max  -  los 


(l,num_los+l)  ; 
(l,num_los+l)  ; 

(1,  num_los+l)  ; 
(l,num_los+l)  ; 
(l,num_los+l)  ; 
(l,num_los+l)  ; 
(l,num_los+l)  ; 

(1,  num_los+l) ; 
zeros  (l,num__los+l) ; 
niin)/num  los; 


accel  =  [.79,  1.0,  1.385] ; 

min_vl_over_v2_accel  *  le9*ones  (max (size  (accel) )  ,num_los+l) ; 
eta_at_min_accel-zero3  (max  (size  (accel) ) ,  num_los+l) ; 

for  i  **  l:num_los+l; 
i 

los(i)  *  los_min  +  (i-l)*d_los; 

%  Find  max  vl/v2  for  separate  solution  components.  No  acceleration  limit, 
for  eta  ■=  los  (i)  :  .  01  :pi-epss/2; 

% - 
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%  max  vl/v2  for  separate  solution  components.  No  acceleration  limit. 
vl_over_v2  -  (sin (eta) /eta)  *  abs(sin  (eta  -  los(i))); 
if  (  vl_over_v2  >  max_vl_over_v2  (i)  ) 

max_vl_over__v2  (i)  ■=  vl_^over_v2; 
eta_atjmax(i)  «  eta; 
end 

% - 

end 

%  Find  min  vl/v2  that  still  hits  acceleration  limit  on  the  pursuer. 

%  The  min  switches  branches  (of  intercept  surface)  when  vl/v2  •=  sin(los/2) 

%  avoid  the  2nd  branch  (with  eta2“pi)  by  running  eta2  from  0  to  (los+px)/2 
for  ia  *  1 :max (size (accel) ) ; 
a  -=  accel  (ia);  %  accel_2/ (2*V2*V2/rl2)  ; 

for  eta  -  epss : . 01 : (3*los (i)  +  pi)/4 

% - 

vl  over  v2  accel~norm{  [a; 0]  “Sin  (eta)  *  [cos  (eta*“los  (i) )  ;  sin  (eta^los  (i) )  ] ) /eta# 

if  (  vl  over__y2^accel  <  min^vl^over _ y2_^accel  (la^  i)  ,  ) 

min_vl__over_y2_accel  (ia,  i)  *  vl^over^v2_^accel; 
eta_at__min_accel  (ia,  i)  *=  eta; 
end 
% 

% - - 

end  %  end  of  eta  loop 

if  (  abs  (sin  (eta_at_min__accel  (ia,  i) ) )  >  a  )  %  accel__2  >  a2  when  vl*0 

min_vl_over_v2_accel (ia,i)  **  0; 
end 
% 

end  %  end  of  ia  loop 

%max  vl/v2  .724*  (  1  -  sin (los (i) /2)  ); 

% 

lower_bound(i)  »  .724*  (  0.99  -  sin (los (i) /2)  ); 

middled)  -  .724*(  1.00  -  sin  (los  (i) /2)  ); 

uppe rebound (i)  *  .724*(  1.03  “  sin (los (i) /2)  ); 

% 

difference (i)  “  .033  *  exp (- . 8*los (i) )  *  sin (1 . 6*los (i) ) ; 

%difference  (i)  -  .015  *  sin  (2*los  (i) )  /  (1+ (los  (i) /1 .5) '"6; 

eta_at_max_{i)  ^  (  pi  -  asin  (max_vl_over_v2  (i) )  +  los(i)  )/2; 

vl  over  v2  iter  con(i)  **  (sin(eta _ at^max _ (i))/eta _ at__max__(i)  )  /  ••• 

sqrt(T  +'’(l/eta_atjcnax_(i)  -  1/tan  (eta_at_jnax_(i) )  ) '^2  ); 

end 

%plot  (los,max_vl_over_v2,  los,  vl_over_v2_iter_con)  %  need  max  first 

plot (los,max_vl_over_v2,  los, uppe rebound,  los, lower_bound) 

title('max  vl/v2  that  satisfies  separate  solution  conponent  bound' ) 

ylabel('.724  {  .99  -  sin(los/2)  )  <  vl/v2  <  .724  (  1.03  -  sin(los/2)  )') 

xlabel('los') 

axis ( [0,pi, 0,1]) 

pause 

%  plot  (los, max_vl__over_v2  —  middle,  los, difference) 

%  plot  (los,  max_vl_over_v2-lower_bound) 

%  plot  (los,  max_vl_over_v2-upper_bound) 

%  plot  ( los  ( 1 : num_los ) ,  et a_at_max  ( 1 : num_los )  -eta__at_jnax_ ( 1 : num^los ) ) 

plot  (eta_at_max_,max_vl_over_v2,  eta_at_max_,  vl_over_v2_iter_con) 
ylabeK'max  vl/v2  separate  solution.  vl/v2  iteration  converge') 
xlabeK'eta  at  max') 
pause 

junk  *  zeros (max (size (accel) ), num_los+l) ; 
for  ii  «  1 :max(size (accel) ) 
for  ij  **  l:num_los+l 
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%junk(ii,ij)  *  min (accel (ii) /pi/  (accel(ii)  ~  sin (los (i j) ) ) /los (i j)  ); 
junk(ii/ij)  -  (accel(ii)  -  sin (los (i j) ) ) /los (i j)  ; 
end 
end 

%plot  (loS/  sin  ( .  5*los)  /  lo3/min_vl_over_v2_accel/  los/  junk,  loS/Xtiax_vl_over_v2) 
plot  (loS/max__vl_over_v2,  loS/min_vl__over_v2_accel  ) 

titletextl  “  'max  vl/v2  to  guarantee:  (Solid)  Separate  sol  conponents, 

titletext2  -  ' (Dotted)  accel2  <  ' ; 

titletext3  -  sprintf (' [%4 .2f ,  %4.2f/  %4,2f]'/  accel (1) , accel (2) , accel (3) ) ; 
title ( [titletextl,  titletext2,  titletext3] ) 

ylabel (' vl/v2  *  min_over_e  (|[accel2;0]  ~  sin (e) * [cos (e-los) ; sin (e-los) ] I  /  e)') 
xlabel (' los' )  - 

axis({0,pi,0,l] ) 


save  vlv2  vs  los 
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ABSTRACT 

In  this  paper  we  focus  on  the  interrelationship  be¬ 
tween  state  estimation  filtering  errors  and  their  ultimate 
impact  upon  guidance  algorithm  performance.  We  com¬ 
pare  the  performance  of  proportional  navigation  guidance 
and  robust  guidance  versus  fuel  optimal  guidaince  in  a  sim¬ 
ple  two-dimensional  missile/target  engagement.  We  find 
that  with  perfect  state  estimation  both  proportional  nav¬ 
igation  and  robust  guidance  achieve  small  miss  distances 
and  come  very  close  to  matching  the  ideal  fuel  optimum 
response.  However,  in  the  presence  of  realistic  state  es¬ 
timation  errors  their  performance  degrades  sharply.  We 
explore  the  magnitude  of  this  performance  degradation 
and  its  impact  on  overall  missile  system  performance. 

INTRODUCTION 

In  a  typical  missle  system  design  active  or  passive 
seeker  information  is  input  into  a  filter  to  obtain  state 
estimates  of  the  position,  velocity  and  acceleration  of  the 
target.  This  information  is  then  input  into  a  guidance 
algorithm  which  uses  it  and  a  knowledge  of  intercept  dy¬ 
namics  to  ultimately  produce  a  missUe  acceleration  com¬ 
mand  profile. 

From  our  past  experience  with  missile  system  de¬ 
sign  we  have  noticed  a  clear  need  for  understanding  the 
impact  of  state  estimation  errors  on  guidance  system  per¬ 
formance.  These  two  subsystems  are  typically  designed 
independently  by  different  engineers.  The  filter  designer 
attempts  to  obtain  the  best  possible  state  estimates,  op¬ 
timizing  a  complex  tradeoff  between  state  estimation  er¬ 
rors,  complexity  of  filter  designs,  input  noise  characteris¬ 
tics  and  computing  power  requirements.  Simultaneously, 
the  guidance  law  designer  conducts  his  design  assuming 
that  he  will  receive  near  perfect  state  information  and  as¬ 
suming  that  his  algorithm  is  implementable  by  existing 
missile  system  hardware  with  little  or  no  modification. 
His  main  concern  is  to  tradeoff  fuel  consumption  versus 
miss  distance  to  achieve  a  satisfactory  guidance  law. 

The  main  drawback  of  the  above  scenario  is  that  al¬ 
though  the  filtering  and  guidance  subsystems  are  each  de¬ 
signed  independently,  the  accuracy  of  the  state  estimates 
has  a  large  impact  upon  guidance  law  performance. 


To  illustrate  this  impact  we  will  compare  the  per¬ 
formance  of  three  different  guidance  algorithms:  fuel  op¬ 
timal  guidance,  proportional  navigation  guidaince  and  ro¬ 
bust  guidance,  using  the  simple  two-dimensional  guidance 
problem  shown  in  Figure  1.  Here,  the  target  and  the  mis¬ 
sile  are  represented  by  point  mass  models  where  the  tairget 
is  denoted  by  the  letter  T  and  the  missile  is  denoted  by 
the  letter  M.  The  relative  positions  of  the  missile  and  tar¬ 
get  are  represented  with  respect  to  a  xy-coordinate  fraime 
whose  origin  is  always  coincident  with  the  location  of  the 
target.  In  this  coordinate  frame  the  tairget  has  zero  veloc¬ 
ity  and  aicceleration  and  is  adways  located  at  the  origin. 
Conversely,  the  missile  always  begins  a  relative  distajice 
from  the  target  (origin)  and  is  also  moving  relative  to 
the  target  (origin)  with  some  velocity  and  acceleration. 
To  further  simplify  our  example,  we  assumed  no  gravity 
effects  (i.e.  an  exoatmospheric  engagement). 

GUIDANCE  ALGORITHMS 

In  this  section  we  will  briefly  discuss  details  of  the 
three  different  guidance  aJgorithms.  The  first  algorithm, 
fuel  optimal  guidance,  is  quite  similar  to  bang-bang  con¬ 
trol  and  produces  a  theoretically  achievable  optimum  level 
of  performance.  The  second  algorithm,  proportion  navi¬ 
gation  guidance,  is  the  most  commonly  used  guidance  al¬ 
gorithm  in  practice  today  due  to  its  combination  of  good 
performance  and  easy  implementation.  The  third  algo¬ 
rithm,  robust  guidance,  is  based  on  some  recently  devel¬ 
oped  ideas  [Ting  (1992)]  designed  to  enhance  the  robust¬ 
ness  of  the  guidance  algorithm  to  state  estimation  errors. 
It  requires  more  computation  than  proportional  naviga¬ 
tion  guidance  and  has  not  been  implemented  in  practice. 
However,  it  has  shown  some  good  properties  in  some  pre¬ 
liminary  simulation  studies. 

Riel  CJptimal  Guidance 

To  assess  the  performance  impact  of  estimation  er¬ 
rors  and  different  guidance  algorithms  on  overall  system 
performance,  it  is  important  to  quantify  the  optimum 
achievable  performance  as  a  benchmark.  In  a  realistic 
missile  system  this  requires  hitting  the  target  with  a  min¬ 
imum  of  fuel  expenditure,  while  simultaneously  adhere- 
ing  to  hardware  imposed  acceleration  limits.  To  this  end, 
consider  the  linear  system 

y  =  Vy  (1) 

Vy  —  a  (2) 

where  the  maximum  achievable  acceleration  is  con¬ 
strained  by  a  magnitude  limit  Umax.  The  fuel  opti¬ 
mal  guidance  problem  can  now  be  formulated  as  follows: 
Given  any  initial  condition  y(0)  =  y©  and  t;y(0)  =  Vy^ 
and  a  fixed  final  time  ip  find  the  acceleration  profile  a(t) 


which  drives  the  system  to  the  final  condition  y(tF)  =  0 
and  =  free  while  minimizing  the  cost  functional 

•^  =  /  II  “(0  II  dt.  (2) 

Jo 


concept  of  this  guidance  law  can  be  summarized  as  fol¬ 
lows:  Accelerate  fully  in  either  the  positive  or  negative 
direction  (whichever  is  appropriate)  until  the  zero  effort 
miss  distance  is  zero  and  then  coast  in  to  hit  the  target. 

Proportional  Navigation  Guidance 


This  is  a  standard  optimal  control  problem  which 
can  be  solved  via  a  straightforward  application  of  the 
maximum  principle  [Athans  and  Falb  (1971)].  The  result¬ 
ing  optimal  control  law  is  an  ‘accelerate  and  coast’  single 
switch  sequence  of  accelerations  of  the  form  (— amox,0) 
or  (+amax,0)  depending  on  where  the  initial  condition 
(yo,  Vy^)  lies  in  the  (y,  Vy)- plane.  The  detailed  optimal 
control  law  is  given  by: 


Case  1:  If  Vy^  ^  ^  : 


«(<)  = 


{ 


-1 

~0 


for  0  <  <  <  1 

for  f  J 


where 


(3) 


Z  —  (Vj/o  ^moxfjF' )“l“ \/^(Umaxf F"  t7y^ )^  {~^^maxyo  “b  ^y©) 

(4) 

and 


Umax 


(5) 


Proportionzd  navigation  is  the  most  commonly  used 
navigation  algorithm  in  practice  today.  In  this  algorithm 
the  missile  normal  acceleration  command  is  given  by 

ON  =  KXVc  (11) 

where  K  is  a  constant  (typically  between  2  and  5),  A  is 
change  of  the  Jin^ofTsighL^^^  betwwn  the, 
missile  and  target  and  K  is  the  relative  closing  velocity 
between  the  missile  and  target.  Mathematically,  this  al¬ 
gorithm  can  be  obtained  by  solving  the  following  linear 
quadratic  optimization  problem  [Bryson  and  Ho  (1973)]. 
Consider  the  linear  system  given  in  Equation(l)  with  ini¬ 
tial  conditions  y{io)  =  yo  and  t;y(to)  =  Vyo*  Suppose  ip 
is  fixed  and  we  want  to  find  the  acceleration  profile  a{i) 
which  minimizes  the  cost  functional 

J  =  .5(ciy(t/')  +  C2t;y(t^)^) -f  .5  /  a(t)^d<.  (12) 


Case  2:  If  Uy^  =  —  ^  then 

a(t)  =  {  0  /or  0<t<tF  }. 

Case  3:  If  Vy^  <  — ^  then 


where 


a(t)  = 


{ 


+1 

+0 


for 

for 


0<t<t,  1 

ts  <t<tF  J  ■ 


(6) 

(7) 


2  =  {Vyo+OmaxtF)-y/(amaxtF  -  (Vy„  -  2amaiJ/o) 

(8) 

and 


^ 

Umax 


(9) 


It  is  clear  that  the  fuel  optimal  guidance  law  is  gov¬ 
erned  by  the  location  of  the  initial  condition  (yo,Vy^)  rel¬ 
ative  to  the  line 


(10) 


in  the  (y,  Vy)-plane.  If  the  initial  condition  lies  on  this 
line  then  the  optimal  guidance  law  is  zero  acceleration 
for  all  time  because  the  vehicle  is  already  on  a  homing 
trajectory  with  the  target.  If  the  initiad  condition  lies 
above  this  line  then  the  optimal  guidance  law  is  to  apply 
maximum  negative  acceleration  from  time  t=0  until  the 
switching  time  t=t,  and  then  to  apply  zero  acceleration 
for  the  duration  of  the  flight.  Finally,  if  the  initial  con¬ 
dition  lies  below  this  line  then  the  optimal  guidance  law 
is  to  apply  maximum  positive  acceleration  from  time  t=0 
until  the  switching  time  i—t,  and  then  to  apply  zero  ac¬ 
celeration  for  the  duration  of  the  flight.  Essentially,  the 


Given  any  set  of  constants  ci  and  C2  we  can  eeisily  solve 
for  the  optimal  acceleration  u*(<)  as  a  function  of  ci,  C2, 
y{t)  and  Vy(<).  If  we  modify  the  cost  function  J  to  em¬ 
phasize  only  terminal  position,  i.e.  we  let  ci  approach 
infinity  and  let  C2  approach  zero,  then  u*(<)  reduces  to 
the  proportional  navigation  control  law. 

Proportional  navigation  exhibits  excellent  perfor¬ 
mance  under  ideal  conditions.  However,  it  does  not  ad¬ 
dress  some  of  the  implementation  issues  which  may  arise 
in  a  real  life  missile  system  application.  For  instance,  in 
pro-nav  guidance  the  desired  set  of  normal  acceleration 
commands  is  allowed  to  take  on  an  unbounded,  continu¬ 
ous  set  of  values.  Clearly,  no  hardware  implementation 
can  achieve  infinite  acceleration,  so  an  exce^ively  large 
acceleration  commands  can  produce  temporary  satura¬ 
tions.  Also,  continuouly  valued  acceleration  commands 
are  incompatible  with  the  increasingly  common  ON/OFF 
or  discretely  throttleable  thrusters.  This  complication  is 
often  treated  by  implementing  corrective  schemes  such  as 
pulse- width  modulation  (PWM),  to  approximate  a  con¬ 
tinuous  signal  by  a  sequence  of  discrete  signals.  The  con¬ 
sequences  of  such  saturations  and  approximations  will  of 
course  be  reflected  in  degraded  performance. 

Pro-nav  guidance  also  does  not  address  the  issue  of 
minimizing  fuel  usage  under  imperfect  conditions.  In  our 
past  experience,  pro-nav  guidance  can  be  extremely  sensi¬ 
tive  to  estimation  errors  which  may  result  in  an  excessive 
amount  of  thruster  activity.  In  simulation  runs  this  is  ev¬ 
ident  by  near  simultaneous  firing  of  a  thruster  in  opposite 
directions,  thus  nearly  nullifying  the  effect  of  both  firings. 
This  phenomenon  can  be  adieviated  somewhat  by  passing 


the  pro-nav  guidance  command  output  through  a  dead- 
zone  filter.  The  size  of  the  ‘optimal’  deadzone  is  typically 
determined  experimentally  and  is  dependent  upon  other 
characteristics  of  the  overall  system. 

A  Robust  Guidaxice  Algorithm 

An  approach  for  developing  a  robust  guidance  algo¬ 
rithm  was  introduced  in  [Ting  (1992)].  Despite  its  skele¬ 
ton  development,  this  algorithm  is  presented  because  it 
demonstrates  some  of  the  potential  performance  improve¬ 
ments  available  by  incorporating  system  integration  con¬ 
siderations  into  subsystem  design.  The  novel  aspect  of 
this  algorithm  is  that  its  acceleration  command  output  is 
directly  affected  by  the  magnitude  of  the  state  estimation 
errors. 

The  robust  guidance  approach  consists  of  three  main 
steps:  1)  A  Zero  Effort  Miss  distance  calculation,  2)  A 
computed  miss  distance  error  radius  R  to  account  for  fil¬ 
ter  estimation  errors  and  3)  A  miss  distance  deadzone 
concept.  The  Zero  Effort  Miss  distance  (ZEM)  is  com¬ 
putable  at  any  point  in  a  missile-target  engagement.  It 
represents  the  distance  by  which  the  missUe  will  miss  the 
target  assuming  that  the  missile  acceleration  is  zero  for 
the  duration  of  its  flight.  For  the  robust  guidance  algo¬ 
rithm  concept,  it  is  necessairy  to  compute  not  only  the 
ZEM  but  also  the  sensitivity  of  the  ZEM  with  respect  to 
the  target  state  estimates  of  position,  velocity  and  accel¬ 
eration. 

The  main  idea  behind  the  robust  guidance  approach 
b  embodied  in  the  mbs  distance  deadzone  concept.  This 
concept  can  loosely  be  stated  as  follows.  Given  the  com¬ 
puted  ZEM  and  the  error  radius  R,  if  the  actual  ZEM 
could  in  fact  be  zero  then  do  not  change  the  course  of  the 
mbsile.  However,  if  the  actual  ZEM  cannot  be  zero,  then 
output  commands  which  place  the  missile  on  an  intercept 
trajectory  with  its  target.  In  contrast  to  most  guidance 
algorithms  which  output  acceleration  commands,  the  ro¬ 
bust  guidance  algorithm  outputs  commands  in  the  form 
of  a  velocity  correction.  The  miss  dbtance  deadzone  con¬ 
cept  can  now  be  restated  as  follows.  If  the  magnitude 
of  the  computed  ZEM  b  small  then  set  the  velocity  cor¬ 
rection  to  zero.  If  the  magnitude  of  the  computed  ZEM 
b  large  then  set  the  velocity  correction  equal  to  a  value 
which  should  correct  the  ZEM  to  zero.  In  this  algorithm 
the  boundary  between  large  and  small  is  governed  by  the 
computed  mbs  distance  error  radius  R. 

In  thb  paper  our  robust  guidance  algorithm  will 
only  be  required  to  output  y-component  velocity  correc¬ 
tions.  In  thb  context,  the  mbs  distance  deadzone  concept 
can  be  interpreted  as  shown  in  Figure  2.  Here,  the  val¬ 
ues  along  the  horizontal  zxis  represent  various  values  of 
ZEM.  For  instance,  suppose  the  computed  value  of  ZEM 
=  ZEMo.  If  the  computed  miss  distance  error  radius  R 
=  Ri  then  the  y-velocity  component  correction  is  set  to 
zero.  However,  if  the  miss  distance  radius  R  =  i?2  then 


the  y-velocity  component  correction  b  set  to  As'EKf'- 

A  Vy 

THE  IMPACT  OF  ESTIMATION  ERRORS 

To  assess  the  relative  performance  of  various  guid¬ 
ance  algorithms  we  used  a  two-dimensional  simulated  en¬ 
gagement  between  a  mbsile  and  a  target  as  described  ear¬ 
lier  and  as  shown  in  Figure  1.  The  initial  conditions  of  thb 
simulation  were  as  follows.  The  missile  and  target  began 
100,000  units  apart  in  the  x-direction  with  a  relative  mis¬ 
sile/ target  velocity  of  10,000  units/sec  in  the  x-direction. 
We  assumed  that  neither  the  mbsile  nor  the  target  were 
accelerating  in  the  x-direction  and  thus  each  simulation 
run  had  a  fixed  duration  of  10  seconds.  The  relative  y- 
dbplacement  and  velocity  of  the  missile  and  target  were 
specified  at  the  beginning  of  each  simulation  run.  It  was 
assumed  that  the  target  had  zero  y- acceleration  and  the 
mbsile  y- acceleration  was  provided  by  the  output  of  the 
guidance  algorithm. 

We  conducted  two  separate  types  of  analyses.  The 
first  type  was  called  single  run  analysb  and  involved  ana¬ 
lyzing  the  performance  of  each  of  the  guidance  adgorithms 
based  on  mbs  dbtance  and  fuel  consumption  for  the  same 
engagement  scenario.  The  second  type  was  called  initial 
condition  system  analysb  and  involved  analyzing  each  of 
the  guidance  algorithms  based  on  the  set  of  initial  en¬ 
gagement  scenarios  which  were  hittable  given  a  fixed  set 
of  system  char  act  erbtics. 

Single  Run  Analysis 

We  ran  a  variety  of  simulations  of  the  engagement 
described  above  using  the  fuel  optimal,  proportional  nav¬ 
igation  and  robust  guidance  algorithms  described*  earlier. 
We  shall  describe  our  analysb  for  an  engagement  where 
the  initial  relative  y-dbplacement  was  -3000  units  and  the 
initial  relative  y-velocity  was  0  units/sec. 

We  first  considered  the  case  where  the  estimation 
filter  produced  perfect  state  estimates,  Thb  gave  us  a 
chance  to  assess  how  close  the  pro-nav  guidance  and  ro¬ 
bust  guidance  algorithms  came  to  achieving  the  true  ‘op¬ 
timal’  solution.  The  results  of  these  simulation  runs  are 
summarized  in  Figures  3-4,  Figure  3  shows  the  mbs  db¬ 
tance  performance  of  all  three  algorithms.  Clearly,  they 
all  demonstrated  acceptable  performance  in  hitting  the 
target.  Figure  4  shows  the  total  amount  of  thruster  firing 
time  for  the  duration  of  the  simulation.  As  expected,  the 
fuel  optimal  guidance  produced  the  minimum  amount  of 
thruster  firings.  The  robust  guidance  required  roughly  6 
percent  more  thruster  firings  while  pro-nav  guidance  re¬ 
quired  roughly  11  percent  more  thruster  firings. 

The  rationale  behind  the  variations  in  fuel  usage  b 
evident  in  Figures  5-7  which  detail  each  of  the  three  guid¬ 
ance  algorithms  output  acceleration  commands.  The  opti¬ 
mal  fuel  guidance  output  acceleration  command  b  shown 
in  Figure  5.  From  this  figure  we  can  see  that  the  fuel 


optima]  strategy  is  to  turn  on  the  thrusters  initially  for 
as  long  as  necessary  to  put  the  missile  on  an  intercept 
trajectory,  i.e.  drive  ZEM  to  zero,  and  then  shut  them  off 
for  the  remainder  of  the  run.  Heuristiccdly,  this  is  clearly 
the  fuel  optimal  solution  because  a  fixed  amount  of  ac¬ 
celeration  change  at  the  beginning  of  the  run  will  result 
in  a  much  larger  total  position  change  over  the  course  of 
the  run  than  the  same  amount  of  acceleration  change  ap¬ 
plied  at  a  later  time.  From  Figures  6  and  7  we  can  see 
that  both  the  robust  and  pro-nav  guidance  algorithms 
produce  nonzero  acceleration  commands  throughout  the 
latter  stages  of  the  run.  It  is  interesting  to  note  that 
in  this  case  the  robust  guidance  acceleration  profile  more 
closely  matches  the  theoreticad  optimum  than  does  the 
pro-nav  guidance  acceleration  profile.  This  is  especially 
true  at  the  beginning  of  the  run  when  the  robusTguidance 
law  maintains  full  acceleration  for  a  longer  time  than  the 
pro-nav  guidance  law.  Consequently,  the  ZEM  associated 
with  the  robust  guidance  law  is  initially  reduced  faster 
than  the  ZEM  associated  with  the  pro-nav  guidance  law. 
This  can  be  seen  in  Figure  4. 

We  next  considered  the  case  with  imperfect  state 
estimates.  This  was  ziccomplished  by  corrupting  each  of 
the  state  values  with  random  noise  with  zero  mean  and 
standard  deviation  equal  to  5  percent  of  the  nominal  state 
value.  The  results  of  these  simulation  runs  are  summa¬ 
rized  in  Figures  8-11.  As  seen  in  Figure  8,  all  three  guid¬ 
ance  algorithms  were  still  able  to  achieve  an  acceptably 
small  miss  distance.  However,  from  Figure  9  we  can  see 
that  the  associated  effort,  i.e.  thruster  firings,  increased 
sharply  over  the  perfect  state  estimation  case.  The  es¬ 
timation  error  caused  the  pro-nav  guidance  algorithm  to 
increase  thruster  firings  by  40  percent  while  the  robust 
guidance  algorithm  increased  thruster  firings  by  23  per¬ 
cent.  These  increased  firings  are  clearly  evident  in  Figures 
10  and  11  where  we  see  persistent,  near  coincident,  pos¬ 
itive  and  negative  thruster  firings  throughout  the  latter 
stages  of  the  run.  Such  firings  nearly  nullify  each  other’s 
effect  and  the  resulting  small  net  change  in  the  missile’s 
trajectory  indicate  that  the  guidance  law  is  overracting 
to  the  state  estimation  error.  In  our  example,  the  robust 
guidance  law  is  less  sensitive  to  this  error  than  the  pro- 
nav  guidance  law,  and  thus  its  performance  degrades  less 
sharply.  Nevertheless,  by  continuing  to  increase  the  mag¬ 
nitude  of  the  state  estimation  error,  we  can  quickly  reach 
a  situation  where  neither  guidance  <dgorithm  is  capable 
of  hitting  the  target. 

Initial  Condition  System  Analysis 

An  alternative  and  perhaps  more  meaningful  ap¬ 
proach  to  assessing  the  interrelationship  between  estima¬ 
tion  filtering  errors  and  guidance  algorithm  performance 
is  to  determine  the  impact  of  estimation  errors  upon  the 
set  of  initial  conditions  from  which  the  desired  target  is 
ultimately  reachable.  To  demonstrate  this  approach,  we 
considered  the  system  given  in  Figure  1  and  assumed  a 
ten  second  flight  time.  We  assumed  that  the  target  had 


a  radius  of  2  units  (thus  we  interpreted  miss  distances 
of  2  units  or  less  as  a  hit)  and  assumed  that  the  vehicle 
was  equipped  with  enough  fuel  to  provide  six  seconds  o£ 
maximum  acceleration.  Within  this  framework  we  used  a 
combination  of  analytical  techniques  and  computer  sim¬ 
ulations  to  determine,  for  each  guidance  algorithm  and 
for  a  variety  of  estimation  filtering  error  conditions,  sets 
of  initial  conditions  which  resulted  in  a  successful  target 
intercept. 

The  overall  results  of  our  findings  are  shown  in  Fig¬ 
ure  12.  The  axea  between  each  pair  of  lines  represents 
the  sets  of  initial  conditions  from  which  our  vehicle  can 
reach  the  target  in  each  of  the  following  five  situations: 
1)  Using  the  fuel  optimal  guidance  law  with  no  state  es¬ 
timation  errors  Using  the  pro-hav-guidahce daw- 
no  state  estimation  errors  3)  Using  the  robust  guidance 
law  with  no  state  estimation  errors  4)  Using  the  pro-nav 
guidance  law  with  five  percent  state  estimation  errors  5) 
Using  the  robust  guidance  law  with  five  percent  state  esti¬ 
mation  errors.  Due  to  the  large  scale  involved  the  results 
may  be  difficult  to  interpret. 

To  facilitate  interpretation  we  displayed  the  fourth 
quadrant  of  Figure  12  separately  in  Figure  13.  For  the 
associated  situation,  each  of  these  lines  represents  the  the 
boundary  of  the  set  of  initial  conditions  from  which  the 
missile  can  hit  the  target.  From  this  diagram  we  can 
see  that  even  with  perfect  state  information  neither  the 
pro-nav  nor  the  robust  guidance  law  can  match  the  per¬ 
formance  of  the  fuel  optimal  guidance  law.  For  instance, 
if  the  vehicle  and  target  have  zero  initial  relative  cross¬ 
range  velocity  then  the  fuel  optimal  guidance  law  exhibits 
a  crossrange  intercept  capability  of  4,130  units.  For  the 
robust  guidance  law  and  the  pro-nav  guidance  law  these 
values  are  4,030  units  and  4,000  units  respectively.  These 
differences  are  relatively  small  with  the  robust  guidance 
law  having  only  2.42  percent  less  crossrange  position  ca¬ 
pability  given  a  zero  initial  crossrange  velocity  than  the 
fuel  optimal  guidance  law.  The  pro-nav  guidance  law  was 
slightly  worse  with  3.15  percent  less  capability  under  these 
same  conditions.  However,  it  should  be  noted  that  these 
are  best  case  results  and  that  the  crossrange  intercept 
performance  gaps  widen  as  the  magnitude  of  the  initial 
crossrange  velocity  increases.  For  instance,  if  we  assume 
that  the  vehicle  has  an  initial  y- velocity  of  -200  units/sec 
relative  to  the  target  then  the  fuel  optimal  guidaince  law 
exhibits  a  crossrange  intercept  capability  of  2,130  units. 
For  the  robust  guidance  law  and  the  pro-nav  guidance  law 
these  values  are  2,035  units  and  2,000  units  respectively. 

-  Thus,  for  this  nonzero  value  of  initial  crossrange  velocity 
the  performance  gaps  have  increased  to  4.46  percent  less 
crossrange  position  capability  for  the  robust  guidance  law 
than  the  fuel  optimal  guidance  law  and  6.10  percent  less 
crossrange  position  capability  for  the  pro-nav  guidance 
law  than  the  fuel  optimal  guidance  law. 

From  Figure  13  it  is  clear  that  the  performance  of 
both  the  pro-nav  and  the  robust  guidance  algorithm  are 
both  greatly  affected  by  the  presence  of  five  percent  state 
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estimation  errors.  In  this  case  the  robust  guidance  law 
now  has  15.25  percent  less  crossrange  position  capability 
given  zero  initial  crossrange  velocity  than  the  fuel  optim2J 
guidance  law.  The  pro-nav  guidance  law  fares  even  worse 
with  24.94  percent  less  crossrange  position  capability  un¬ 
der  these  same  conditions.  Similar  to  the  results  presented 
with  no  state  estimation  errors,  these  are  best  case  results 
and  the  crossrange  intercept  performance  gaps  widen  as 
the  magnitude  of  the  initial  crossrange  velocity  increases. 

Comparison  of  Results 

In  both  of  the  analyses  presented  above  we  saw  that 
the  performance  degradation  of  the  pro-nav  and  robust 
guidance  laws  due  to  state  estimation  error  is  very  signif¬ 
icant  when  compared  to  the  performance  gaps  between 
the  pro-nav  and  robust  guidance  law  and  the  fuel  op¬ 
timal  guidance  law  with  no  state  estimation  error.  We 
discounted  the  results  of  any  specific  single  run  analysis 
because  the  relative  performance  of  the  guidance  algo¬ 
rithms  can  diifer  depending  on  the  specifics  of  the  engage¬ 
ment  condition.  However,  we  feel  that  the  initial  condi¬ 
tion  system  analysis  presents  a  relatively  clear  indication 
of  performance.  From  this  analysis,  note  the  three  nearly 
coincident  lines  in  Figure  13,  we  can  see  that  relatively  lit¬ 
tle  perform2aice  (from  the  theoretical  optimum)  is  lost  in 
implementing  either  the  pro-nav  or  robust  guidance  algo¬ 
rithm  with  perfect  state  information.  Conversely,  a  large 
performance  gap  exists  when  either  algorithm  is  imple¬ 
mented  with  a  small  level  of  state  estimtion  error. 

The  main  intent  for  including  the  robust  guidance 
algorithm  was  not  to  advertise  it  as  a  replacement  for  pro- 
nav  guidance.  Instead,  it  was  included  to  demonstrate 
that  it  is  indeed  possible  to  improve  upon  the  performance 
of  pro-nav  guidance  in  the  presence  of  state  estimation 
errors.  By  illustrating  the  size  of  the  performance  gap 
and  the  realistic  possibility  of  closing  this  gap,  we  hope  to 
motivate  further  research  along  these  lines.  We  feel  that 
the  key  to  this  and  future  improvement  lies  in  exploiting 
the  interrelationship  between  estimation  and  guidance  by 
explicitly  incorporating  estimation  errors  into  the  robust 
guidance  algorithm. 

CONCLUSIONS  AND  RECOMMENDATIONS 


timates  can  severely  degrade  guidance  law  performance. 
At  the  same  time,  the  performance  of  the  robust  guid¬ 
ance  law  demonstrates  that  it  may  indeed  be  possible  to 
improve  upon  the  robust  performance  characteristics  of 
pro-nav  guidance.  We  feel  that  future  progress  in  solving 
this  problem  lies  with  a  better  understanding  of  the  inter¬ 
relationship  between  the  state  estimator  and  the  guidance 
algorithm.  This  area  is  currently  under  further  research. 
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In  thb  paper  we  have  attempted  to  demonstrate  the 
significance  of  state  estimation  errors  on  guidance  algo¬ 
rithm  performance.  This  was  accomplished  by  comparing 
the  performance  of  pro-nav  and  robust  guidance  to  ideal 
fuel  optimal  guidance  both  with  and  without  the  presence 
of  state  estimation  errors.  Even  with  perfect  state  infor¬ 
mation  neither  guidance  algorithm  could  achieve  the  the¬ 
oretical  optimum  performance.  However,  the  magnitude 
of  their  suboptimality  provided  a  good  frame  of  reference 
to  assess  the  magnitude  of  performance  degradation  due 
to  imperfect  state  information. 

From  our  studies  it  is  clear  that  imperfect  state  es- 
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ABSTRACT 


Figure  1:  Typical  Miesilc  Divert  Control  Syitem 


In  tlus  paper  we  conndex  the  problem  of  exoat* 
mocpberic  miuile/targei  engagemenU  with  imperfect 
state  information.  We  present  a  newly  developed  robust 
guidance  algorithm  which  achieves  better  hit  performance 
and  less  fael  consumption  than  a  proportional  naviga* 
tion  (pro-nav)  guidance  algorithm  for  certain  engagement 
These  preHxninary  results  lead  ns  to  bebeve 
that  robust  guidance  algorithms  can  eventually  be  deveU 
o]>ed  which  wOl  globally  outperform  existing  guidann  al- 
gozithms  in  dealing  with  uncertainties. 

INTRODUCTION 


This  paper  describes  the  preliminary  develop* 
ment  of  a  robust  guidance  algorithm  for  use  in  exoat- 
mospheric  miiaile/target  engagement  problems.  The  pri¬ 
mary  motivation  for  our  guidance  research  was  twofold. 
First,  we  wanted  to  develop  an  algorithm  which  was 
straightforward  to  implement;  Le.  a  Tnrnimum  of  ad- 
hoc  fixes  and  dependence  on  designer  skill.  Second,  we 
wanted  to  improve  upon  the  performance  of  a  popular 
existing  algorithm,  the  proportional  navigation  (pro-nav) 
guidance  algorithm,  both  in  terms  of  fuel  consumption 
and  hit  probabilities,  hf  uch  of  this  effort  was  directed  to¬ 
wards  improving  upon  the  robustness  of  pro-nav  guidance 
algorithms,  that  is  improving  the  performance  m  the  fo« 
of  uncertainties.  In  our  particular  study  th^  xmoe^a^ 
ties  were  characterised  by  errors  in  the  state  estimates 
used  by  the  guidance  algorithms. 

BACKGROUND  AND  MOTIVATION 


The  linear  and  rotational  motions  of  a  exoat mo- 
sphexic  missile  are  typically  controlled  by  fixing  thrusters. 
The  most  elementary  thrusters  possess  an  ON/OFF  char- 
acteristic.  More  complicated  thrusters  arc  throttleable, 
featuring  a  variable  thrust  level  The  attitude  and  di¬ 
vert  control  thrusters  are  often  subdivided  into  separate 
groups  with  each  group  of  thrusters  controlled  by  separate 
attitude  and  divert  control  systems.  These  systems  are  re¬ 
sponsible  for  feedback  information  about  the 

current  states  of  the  missile  relative  to  its  desired  target 


and  issuing  appropriate  firing  commands.  For  guidance 
purposes,  the  divert  control  s^tem  is  cniaid^^  b^ 
is  responsible  for  controlling  the  non-rotational  motion  of 
the 

A  block  diagram  of  a  typical  missile  divert  control 
system  is  shown  in  Figure  1,  and  its  operation  can  be 
summarised  as  follows.  At  the  beginning  of  each  guid¬ 
ance  cyde,  values  of  the  angular  rates  of  the  vehicle  and 
filter  estimates  of  the  missile’s  linear  position,  velocity 
and  acceleration  relative  to  the  target  xn<^d  are  fed  back 
into  a  guidance,  algorithm.  This  algorithm  processes  this 
information  and  generates  a  vector  of  desired  linear  accel¬ 
eration  commands.  These  linear  acceleration  commands 
are  then  transformed  into  individual  divert  thruster  com¬ 
mands  which  are  applied  to  each  of  the  divert  thrusters. 

If  we  employ  a  standard  pro-nav  guidance  algorithm 
then  the  divert  control  system  design  is  complicated  by 
the  presence  of  ON/OFF  divert  control  thrusters.  Pro- 
nav  guidance  algorithms  are  usually  allowed  to  output 
a  continuous  valued  vector  of  linear  acceleration  com¬ 
mands.  This  results  in  a  continuous  set  of  divert  thruster 
commands  which  are  incompatible  with  ON/OFF  divert 
thrusters.  This  complication  may  be  addressed  by  im¬ 
plementing  a  scheme,  such  as  pulse-width  modulation 
(PWM),  which  converts  a  continuous  valued  input  vec¬ 
tor  of  <Bvert  thwter  coxnmands  into  an  , ON/QFF  ^valued  ^ 
output  vector  of  divert  thruster  commands^ ..  .  ^ 

Proportional  navigation  guidance  also  does  not  ad¬ 
dress  the  issue  of  minimising  divert  thruster  firings.  In 
our  past  experiences  pro-nav  guidance  has  on  occasion 
exhibited  an  excessive  amount  of  divert  thruster  firing 
activity.  This  phenomenon  can  be  treated  by  appending 
a  deadsone  filter  to  the  divert  control  system.  Such  filters 
process  the  guidance  algorithm's  output  vector  of  Hnear 
acceleration  commands  prior  to  their  transformation  into 
individual  divert  thruster  commands.  The  ue  of  the  'op¬ 
timal'  deadsone  is  typically  determined  experimentally. 
Although  this  solution  generally  produces  satisfactory  re¬ 
sults,  we  are  not  satisfied  with  the  absence  of  a  formal 
design  procedure  for  determining  the  sise  of  the  deadsone. 

Our  objective  in  this  paper  is  to  describe  the  prelim¬ 
inary  development  of  a  robust  guidance  algorithm  which 
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four  go&ls  in  conip&riso&  with  n  pto- 

het  guiduice  •clxeme.  Fi«t,  it  produces  discrete  rmlued 
linear  a«cleration  commands  wKicli  are  completely  com¬ 
patible  with  ON/OFF  dirert  thrusters.  Second,  it  aToids 
the  use  of  deadsones  or  determines  them  in  an  analytic 
fashion.  Third,  it  improTci  hit  performance  by  making 
the  terminai  distance  more  robust  to  filter  estimation 
errors.  Finally,  it  reduces  orerall  dirert  fuel  consumption. 

A  PROPOSED  NEW  ROBUST  GUIDANCE 
ALGORITHM 


Main  Ideas 

We  shall  first  explain  the  basic  principles  behind  our 
guidance  algorithm.  The  technical  details  will  be  illus¬ 
trated  a  generic  two  dimenidonal  guidance  problem 

later  in  this  section. 

The  derelopment  of  our  guidance  algorithm  consists 
of  three  main  steps;  1)  A  miss  distance  calculation  3)  A 
tniaa  distance  radius  due  to  filter  estimate  errors  and  3) 
A  rniMM  distance  deadsone  concept.  At  any  point  in  a 
missile/ target  simulated  engagement  we  can  compute  the 
Zero  Effort  distance,  ZEM.  This  represents  the  dis¬ 
tance  by  which  the  missile  will  miss  the  target  assuming 
that  the  acceleration  is  sero,  that  is  no  external 

effort  (force)  is  acting  upon  the  missile,  for  the  duration 
of  its  flight.  For  our  guidance  algorithm,  we  are  inter¬ 
ested  in  not  only  the  ralue  of  ZEM,  but  also  of  the  sensi- 
tirity  of  ZEM  with  respect  to  our  state  estimates  of  the 
relatiTe  linear  components  of  acceleration,  Telocity  and 
displacement.  These  sensitiTities  are  useful  because  they 
represent  the  incremental  change  in  ZEM  which  will  ac¬ 
company  a  unit  incremental  change  in  each  state. 

As  stated  earlier,  one  of  our  foremost  design  objec- 
tires  is  robustness  to  state  estimation  errors.  Thus,  in 
addition  to  computing  a  single  miss  distance  ralue  ZEM, 
we  compute  a  miss  distance  radius  R  due  to  possible  state 
estimation  errors.  This  radius  R  is  computed  as  follows. 
First,  we  determine  a  maximum  magnitude  Altering  error 
bound  for  each  of  the  relatire  acceleration,  Telodty  and 
displacement  states.  Then,  using  the  sensitiTities  of  ZEM 
with  respect  to  these  states,  we  compute  the  maximum 
magnitude  error  in  ZEM  associated  with  the  current  ral- 
ues  of  the  states.  This  maximum  magnitude  error  is  the 
miss  distance  radius  R. 

The  twUs  distance  deadsone  concept  is  Tcry  simple. 
It  determines  the  output  of  the  guidance  algorithm  in  the 
form  of  a  Telodty  correction.  If  the  absolute  Talue  of  the 
computed  "iim  distance  ZEM  is  suflidently  small  then  the 
Tclodty  correction  is  set  to  sero.  Howctct,  if  the  absolute 
Talue  of  ZEM  is  large  then  a  Telodty  correction  is  issued 


-R,  o  ♦R^ 


TTQSetttAX 

Figure  2:  Miss  Distance  Deadsone  Concept 

which  should  correct  the  ZEM  to  sero.  In  our  algorithm 
the  boundary  between  large  and  small  is  giTcn  by  the  miss 
distance  radius  R.  If  the  absolute  Talue  of  ZEM  is  smsUex 
than  R  then  the  guidance  algorithm  outputs  a  Telodty 
correction  of  sero.  HoweTcx,  if  the  absolute  Talue  of  ZEM 
is  greater  than  R  then  the  guidance  algorithm  outputs 
a  Telodty  correction  which  should  correct  the  ZEM  to 
sero.  Analytically,  this  is  done  by  outputting  a  Telodty 
correction  which  is  equal  to  the  current  ZEM  didded  by 
the  sensitiTity  of  the  ZEM  with  respect  to  the  Telodty 
state  Tariable  under  consideration. 

In  this  paper  we  restrict  oni  attention  to  outputting 
only  y-component  Telodty  corrections.  In  this  context, 
the  miss  distance  deadsone  concept  can  be  interpreted 
as  shown  in  Figure  2.  Here,  the  Talues  along  the  hozi- 
Bontal  axis  represent  Taxious  Talues  of  ZEM.  Suppose  the 
computed  Talue  of  ZEM  is  giren  by  ZEM^,  K  the  miss 
distance  radius  R  is  giTen  by  Ri  then  the  y-Telodty  com¬ 
ponent  correction  is  set  to  sero.  EoweTcr,  if  the  miss 
distance  radius  R  is  |pTen  by  then  the  y-Telodty  com¬ 
ponent  correction  is  set  to 

Algorithm  Details 

We  will  now  illustrate  the  details  of  our  guidance  al¬ 
gorithm  the  generic  two  dimensional  guidance  prob¬ 
lem  shown  in  Figure  3.  Here,  the  target  and  the  miisile 
are  represented  by  point  mass  models  where  the  target 
is  denoted  by  the  letter  T  and  the  missile  is  denoted  by 
the  letter  M.  ThordatiTe  positions  of  the  missile  and  tar¬ 
get  are  represented  with  respect  to  a  xy-coordinate  frame 
whose  origin  is  always  coinddent  with  the  location  of  the 
target.  In  this  coordinate  frame  the  target  has  sero  Teloc¬ 
ity  and  acceleration  and  is  always  located  at  the  oxi^. 
ConTctsely,  the  missilr  always  begins  with  a  negatiTe  x- 
component  and  is  represented  by  a  relatiTe  distance  from 
the  target  (origin).  The  missile  is  also  moTing  relatiTe  to 
the  target  (origin)  with  some  Telodty  and  acceleration. 
To  further  simplify  our  problem,  we  assumed  no  graTity 
effects  (Le.  an  ezoatmosphezic  engagement)  and  that  the 
target  maintained  a  constant  acceleration. 

Our  guidance  algorithm  begins  with  a  computation 
of  the  Zero  Effort  Miss.  This  miss  distance  can  be  de¬ 
fined  in  seTcral  different  ways.  For  our  two  dimensional 
problem  we  chose  to  define  the  ZEM  as  the  distance  be¬ 
tween  the  target  and  the  missile  when  the  target  crosses 
the  line  x=0  (i.c.  the  y-component  of  the  trajectory  when 
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Figure  3:  Two  DimcnBional  Guidance  Problem 

At  anj  time  let  ns  denote  tbe  Tilnes  of 

tbe  relatire  z  and  j  components  of  accelerations,  to- 
lodties  and  distances  of  tbe  misdle  and  target  by 

a««,ay«,aM,Vy«,x«  and  y*.  Then  assnming  constant  tar¬ 
get  Bccelexations  and  sero  iniisile  accelexationB,  tbe  fatnxe 
Talnes  of  lelatiTe  z  and  j  components  of  accelerations, 
▼elodties  and  distances  of  tbe  inissile  and  target  at  any 
future  time  i#  +  ii  are  giyen  by 

•■(t#  +  <l)  ~  Omm  (l) 

=  a^  (2) 

+  =  u.#4*  /  a.(<)dt  =  +  (S) 

Ju 

«y(t#  +  t)  =  »,.+  /  a^{i)di  =  tr,o  +  a,.<i  (4) 
e(t«  + 1)  ~  s«  +  j  v«(t)dt  s  +  v»oti  +  .Sa^tx 

Ju 

w 

Vy(t}dt  =  f#  +  e,.ti  +  .5ay•t^ 

W 

From  these  expressions  we  may  compute  tbe  time 
to  go  This  represents  the  length  of  time  before  tbe 
misiOe  crosses  tbe  y-axis.  At  siny  time  t« 
giren  by  solring  the  equation 

*(t»  4*  tf#)  =  *#  +  Vmmtgo  +  ~  0  (7) 

for  ig^.  Two  separate  cases  exist.  If  a^  ^  0,  that  is  if  tbe 
target  is  accelerating  towards  tbe  misnle,  then 


Once  igm  has  been  computed  tbe  miss  distance  ZEM  is 
giyen  by  the  yalue  of  y  at  tgo  or 

ZSJhd  y(t|w)  y«  4*  yy«t|^a  4* 


In  addition  to  the  miss  distance  ZEM  we  are  also 
interested  in  computing  tbe  sensitiyities  of  ZEM  with  re¬ 
spect  to  tbe  relatiye  z  and  y  components  of  acceleration, 
yelocity  and  displacement.  Using  equations  3-10  these 
sensitiyities  can  be  computed  in  a  straightforward  fash¬ 
ion  to  yield 


6.ZEM  _ 
AOy. 

_  dZEM  _  , 

dOya  ■  *• 

(11) 

AZEM 

dZEM  . 

A«,. 

(12) 

AZEM 

Ay. 

dZEM 

dy. 

(IS) 

^  dZE2£  ^  (Vyo  4*  OyotfaX^*  *1* 

Aa««  domm  a««(aaiatfo  4* 

(M) 

/^ZEBd  ^  dZEM  _  '~‘(Vyo  ayatya)tya  flS) 

ASm  dVmm  amp^ammigo  4" 

AZEM  _  dZEM  ^  --(uy  4"  ag»ig9) 

Let  us  denote  our  six  state  estimates  by 
and  dg.  We  must  first  determine  tbe  mag¬ 
nitude  of  possible  estimation  errors  associated  with  each 
of  these  nx  state  yariables.  We  denote  these  error  magm°> 
tudes  by  Ax,  Ay,  Ava,  Avy,  Aa«  and  Aoy.  In  a  true  de- 
egn  problem  these  yalues  could  be  determined  iteratiyely 
by  beginning  with  initial  guesses  with  future  updating 
based  on  actual  simulation  results.  For  our  problem  we 
simply  set  the  magnitude  of  each  estimation  error  equal 
to  a  fixed  percentage  of  tbe  corresponding  state  estimate. 

From  tbe  state  estimation  error  bound  magnitudes 
and  tbe  sexintiyities  giyen  in  Equations  11-16,  tbe  miss 
distance  radius  K  may  be  computed  as 


jr  =t  ——A^  It  +  It  -  ^ ,  Ay  II  + 


AZEU^ 


AZEM  „  AZEM  ^  „  AZEM  .  . 

II  +  II  11  + 11  -Kir 


Altematiycly,  if  <  0,  that  is  if  the  target  is  accelerat¬ 
ing  away  from  tbe  missile,  then 


R  represents  tbe  maximum  possible  uncertainty  in  tbe 
magnitude  of  tbe  ZEM  due  to  state  estimation  errors. 

The  output  of  our  guidance  algorithm  is  an  incre¬ 
mental  yelocity  change  command,  Bvg,  As  stated  earlier, 
we  use  a  yery  elementary  miss  distance  deadsone  concept. 
If  tbe  computed  miss  distance  is  less  than  R,  then  Bvg  is 
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Mt  to  icro.  If  not,  then  Sv^  is  commanded  inch  thnt  it 
conccU  the  next  computed  miss  distance  to  icxo.  Math- 
ematically,  this  is  expressed  as 


f  oif  \\  ZEU  11<  R 

Svy  =  I  |1>R 


TTOSaiSISr.l 


A  major  difference  between  our  guidance  algorithm 
and  pro-naT  guidance  is  the  form  of  the  commanded  out¬ 
put.  Pro-naT  guidance  algorithms  usually  output  linear 
acceleration  commands.  Our  guidance  algorithm  ouputs 
linear  Telocity  incremental  change  commands.  That  is, 
ixutead  of  <v^TnTn»,ndrng  acceleration  in  each  linear  direc¬ 
tion,  our  guidance  algorithm  issues  desired  incremental 
Telocity  changes  in  each  linear  direction. 

As  described  earlier,  pro-naT  guidance  outputs  are 
often  passed  through  deadsone  filters  which  sero  out  all 
input  which  fall  below  a  specified  threshold.  In  contrast  to 
pretious  ad-hoc  methods,  our  guidance  algorithm  was  de¬ 
signed  with  an  automated  procedure  to  handle  the  selec¬ 
tion  of  an  appropriate  deadsone  threshold.  This  thresh¬ 
old  was  selected  such  that  our  guidance  algorithm  output 
incremental  Tclodty  commands  which  were  entirely  com- 
patible  with  ON/OFF  thruster  characteriatics.  This  pro¬ 
cedure  operated  as  follows.  GWen  the  mass  of  the  missile 
and  the  guidance  cycle  rate  we  could  compute  the  maxi¬ 
mum  Telocity  change,  AutwMi  achierablc  during  a 

iiwgV  guidance  cycle.  This  tuIuc  was  them  selected  to  be 
the  threshold  of  our  Telocity  deadsone  filter.  Thus,  our 
true  guidance  algorithm  output  is  computed  as  follows: 

f  Oi/  |15t,I1<  ^ 

gf y  =  <  Aum*  if  >  AsmM  >  .  (19) 

^  —  AVm«a  ^  ^AVfM^a  J 

After  preliminary  testing  with  our  guidance  algo¬ 
rithm  some  additional  features  were  added  to  enhance 
perfbnnancc.  First,  the  miss  distance  deadsone  was  shut¬ 
off  for  the  last  second  of  the  engagement.  Whencrer 
was  one  second  or  less  the  Telodty  correction  was  com¬ 
puted  according  to  the  second  expresabn  of  Equation  18 
without  regard  to  the  inequality  conditions.  Second,  the 
miM  distance  radius  R  giTcn  by  Equation  17  was  sub¬ 
ject  to  upper  and  lower  bounds.  The  upper  bound  was 
appended  because  we  wanted  to  ensure  that  when 
reached  one  second,  ZEM  was  sufficiently  small  so  that 
the  missile  could  successfully  intercept  the  target.  The 
lower  bound  was  appended  to  ensure  that  K  did  not  be¬ 
come  too  small  too  quickly  which  could  result  in  a  ZEM 
response  which  oscillates  about  lero. 

The  upper  and  lower  bounds  on  R  were  set  equal  to 
a  constant  multiple  of  f The  gain  on  the  upper  bound 
was  determined  by  the  relatWe  acceleration  capabilities  of 
the  and  the  target.  For  instance,  since  we  know 


Figure  4:  Block  Diagram  of  Our  New  Guidance  Al¬ 
gorithm 

that  the  distance  deadsone  is  shutoff  when  =  1 
second,  we  want  ZEM  to  be  small  enough  such  that  it  can 
be  reduced  to  sero  in  one  second  or  lets.  The  maximum 
ZEM  correction  achicTable  in  time  t  is  giTcn  by 

tikZEhf  —  .5niur(oymi«jii«  “  •  (^®} 

In  our  cT^plg  AZEM  was  approximately  lOt*.  Thus, 
we  want  the  ZEM  to  be  less  than  30  when  t,o  =  1  second. 
Consequently,  we  set  the  upper  bound  on  R  to  be  30<,o. 

The  lower  bound  on  R  was  not  computed  analyti¬ 
cally.  We  simply  experimented  with  Tarious  gains  unitl 
we  found  one  we  deemed  satisfactory.  For  our  example 
we  settled  on  a  lower  bound  of 

A  block  diagram  illustrating  the  full  computations 
of  our  new  guidance  algorithm  is  shown  in  Figure  4. 

SIMULATION  RESULTS 


We  appHed  oux  new  guidance  algorithm  to  a  two- 
dimemdonal  simulated  engagement  between  a  missile  and 
a  target.  The  iwitial  conditions  of  this  simulation  axe 
characterised  as  follows.  First,  the  missile  and  target  be¬ 
gan  300,000  units  apart  in  the  x-diiection  and  2,000  units 
apart  in  the  y-direction.  Second,  the  missile  began  with 
an  initial  relatiTe  docing  Telodty  of  20,000  units/sec  in 
the  x-direction  and  400  units/sec  in  the  y-dircction.  Fi¬ 
nally,  the  missile  began  with  an  initial  xelatiTe  accelera¬ 
tion  of  50  uniti/sec*  in  the  x-direction  and  -30  units/scc^ 
in  the  y-dkection.  The  initial  relatiTe  acceleration  in  the 
y-direction  was  dexiTcd  from  the  fact  that  the  target  was 
assumed  to  haTc  a  constant  acceleration  of  30  units/«ec^ 
in  the  y-direction  while  the  missile  was  giTcn  sero  initial 
acceleration  in  this  direction. 

We  ran  «TnTi1mtinn«  of  the  engagement  described 
aboTe  using  three  different  guidance  algorithms.  These 
algorithms  were  standard  proportional  naTigation,  stan- 
dard  proportional  naTigation  with  a  deadsone  filter,  and 
an  algorithm  based  on  our  new  guidance  scheme  described 
earlier.  The  sbe  of  the  deadsone  augmenting  the  standard 
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proportionml  naTi^mtion  algoiitlun  wm*  aclccted  bj  trial 
and  error  in  an  attempt  to  achieve  a  compromiae  between 
good  bit  performance  and  minimal  divert  foci  naage. 

The  different  gnidance  algoritbma  were  compared  on 
tbeir  ability  to  perform  in  ibc  acenario  described  above. 
Performance  was  ebaracterised,  by  two  main  factors:  ter¬ 
minal  distance  and  divert  ibmiter  ontime.  Tbe^ 
measures  were  cboaen  because  they  address  two  main 
questions  regarding  any  missile  engagement:  1)  Did  the 

hit  the  target?  and  2)  How  much  foci  do  we  need 
to  put  in  the  missile  so  that  it  can  hit  the  target? 

For  comparison  purposes  we  conducted  two  sets  of 
simulation  runs.  The  ffxst  set  of  runs  were  conducted  as> 
suming  no  state  estimation  errors.  The  second  set  of  runs 
were  conducted  assuming  g  ten  percent  estimation  error 
in  the  relative  x-distanoc  between  the  mis^e  and  target. 
We  could  have  conducted  iiTnulatioB  runs  assuming  es¬ 
timation  errors  in  more  than  one  state,  but  we  felt  that 
the  two  sets  of  runs  described  above  were  sufficient  to 
illustrate  our  results. 

The  resulU  of  the  first  set  of  simulation  runs  (each  of 
the  three  guidance  schemes  run  separately  with  no  state 
estimation  errors)  axe  shown  in  Figure  6.  The  simulation 
results  obtained  using  a  proportional  navigation  guidance 
algorithm  axe  shown  by  the  so&d  black  Hnes.  By  append¬ 
ing  a  deadsone  to  the  proportional  navigation  algorithm 
we  obtained  the  nmulation  results  shown  by  the  smsH 
dotted  Hnes.  Finslly,  the  simulstinu  results  obtsined  us¬ 
ing  the  guidsnee  slgorithm  described  in  this  paper  axe 
shown  by  the  medium  dotted  hnes. 

It  is  dear  that  with  no  state  estimation  errors,  all 
three  guidance  algorithms  adueve  comparaUe  miss  dis¬ 
tance  performance.  In  foct,  the  pro-nav  guidance  and  the 
pro-nav  guidance  with  the  added  deadsone  achieve  nearly 
identical  miss  distances.  -Our  gnidance  algorithm  results 
in  a  sHghtly  larger  terminal  miss  distance,  but  this  dif¬ 
ference  ii  almost  negHgiblc  when  compared  to  the  siie  of 
most  targets. 

Conversdy,  the  three  guidance  schemes  differ  greatly 
in  the  amount  of  divert  thruster  ontime.  This  is  ngnifi- 
cant  because  with  ON/OFF  diivcrt  thrusts 
of  divert  thruster  fad  consumed  is  directly  proportional 
to  the  amount  of  thruster  ontime.  Since  additional  fad 
mcremses  the  weight  of  the  Tnissilr  which  must  be  car^ 
into  space,  it  is  desirable  to  minimise  the  fad  require- 
menu  of  the  mlsdle. 

From  Figure  5  it  is  evident  that  the  pro-nav  g^an« 
ilgotilhm  resulU  in  the  largest  divert  thruster  ontime  (di¬ 
vert  fad  consumption).  This  value  is  reduced  (in  otu  e*P 
ample)  by  approximatdy  fifteen  percent  by  appending  a 
deadsone  to  the  pro-nav  guidance  algorithm.  Howcw, 
by  implementing  the  oui  guidance  algorithm  it  is  possible 


Figure  5:  Simulntion  ResulU  With  No  State  Estima¬ 
tion  Errors 

to  reduce  divert  thruster  ontime  by  more  than  fifty-five 
percent  as  compared  to  a  standard  pro-nav  guidance  al¬ 
gorithm.  Thus,  in  this  case  our  guidance  algorithm  ^ 
greatly  reduce  the  amount  of  divert  thruster  activity  with 
a  very  impact  upon  miss  distance  performsnee. 

In  fact,  this  inpact  is  so  small  that  it  probably  would  not 
change  the  Hkelihood  of  hitting  a  target. 

The  resulU  of  the  second  set  of  simulation  runs  (each 
of  the  three  guidance  schemes  run  separatdy  with  a  ten 
percent  state  estimation  error  in  the  relative  ac-^tance 
between  the  mnA  target)  are  shown  in  Figure 

The  resulU  corresponding  to  the  three  different  guidance 
algorithms  arc  indicated  by  the  same  type  of  Hnes  de¬ 
scribed  for  Figure  5. 

In  this  thc  pro-nav  guidance  algorithm  and 

the  pro-nav  guidance  with  thc  appended  deadsone  agtin 
achieve  nearly  identical  miss  distance  performance.  Un- 
fortnnately,  both  guidance  algorithms  result  in  a  rcla- 
large  terminal  t"***  distance.  The  deterioration 
in  distance  performance  as  compart  to  thc  simu¬ 
lation  with  no  state  estimation  errors  indicates  that  nei¬ 
ther  of  these  two  algorithms  u  particularly  robust  to  state 
estimation  errors.  By  contrast,  our  guidance  algorithm 
achieves  very  good  miss  distance  performance  in  spite  of 
this  state  estimation  error.  A  comparison  of  Figures  5  and 
6  indicates  that  the  terminal  miss  distance  performance 
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Figure  6:  SimulAtion  Results  With  Ten  Percent  X- 
State  Estimation  Errors 


expect  that  on  STcrage  aa  the  magmtnde  of  the  state  es¬ 
timation  errors  increase,  the  percentage  increase  in  fad 
consomptbn  OTcr  the  perfect  information  case  will  also 
increase. 


CONCLUSIONS  AND  RECOMMENDATIONS 


We  haTe  explored  some  new  ideas  for  dereloping 
a  robnst  guidance  algorithm.  These  ideas  were  incorpo¬ 
rated  into  a  first  cat  Tcrsion  of  a  new  goidance  algorithm. 
This  algorithm  performed  faTorably  in  a  nmolated  mis¬ 
sile/ target  engagement  in  comparison  with  two  Tcrsions 
of  standard  pro-naT  goidance.  Howerer,  we  must  cau¬ 
tion  that  these  results  arc  engagement  dependent  and  fa^ 
▼orable  results  are  not  always  obtained.  In  addition,  aL 
though  we  were  able  to  avert  a  majority  of  the  ad-hoc  pro¬ 
cedures  and  ON/OFF  thruster  complications  associated 
with  the  prtvnaT  guidance  algorithm,  we  were  not  able 
to  remove  all  of  the  ad-hoc  procedures  from  our  guidance 
algorithm  design,  Le.  the  selection  of  the  lower  bound  on 
R.  Further  research  is  needed  to  better  understand  the 
fun  benefits  and  drawbacks  of  our  ideas.  It  is  our  hope 
that  this  paper  may  help  to  focus  attention  on  this  prob¬ 
lem  and  stimulate  reseat  into  the  devdopment  of  more 
robust  guidance  algorithms. 


of  our  guidance  algorithm  deteriorates  very  little  in  the 
presence  of  this  state  estimation  error. 

It  is  also  seen  that  the  presence  of  the  state  esti¬ 
mation  did  not  affect  the  relative  fuel  consumption  char¬ 
acteristic  of  the  three  guidance  schemes.  The  pro-nav 
guidance  algorithm  again  required  the  largest  fuel  con¬ 
sumption,  f^wed  by  the  pro-nav  guidance  with  the  ap¬ 
pended  deadsone  and  finally  our  new  guidance  algorithm. 
As  before,  appending  the  deadsone  to  the  pro-nav  guid¬ 
ance  *%OTithm  reduced  divert  thruster  ontime  by  roughly 
fifteen  percent,  while  our  new  guidance  algorithm  reduced 
dirert  thruster  ontime  to  roughly  one  half  of  the  value  as- 
sodated  with  the  pro-nav  guidance  algorithm.  However, 
jiQ  case  the  new  guidance  algorithm  not  only  achieved 
greatly  reduced  fuel  consumption  in  comparison  with  pro- 
nav  guidance,  but  also  achieved  a  significant  advantage  in 
terminal  distance  performance. 

In  the  tranntioa  from  the  perfect  informatioa  case 
to  the  case  with  state  estimation  error,  we  note  that  the 
fuel  consumption  assodated  with  aH  three  guidance  algo¬ 
rithms  increased.  This  is  expected  because  the  presence 
of  state  estimation  errors  leads  to  erroneous  thruster  com¬ 
mands  and  subsequent  thruster  firings.  In  such  dtuations, 
fuel  is  wasted  not  only  on  these  erroneous  firings  them¬ 
selves.  but  in  the  firings  which  must  be  made  to  correct 
the  harmful  effects  caused  by  the  erroneous  firings.  We 
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Fault  Detection  in  the  Presence  of  Modeling 

Uncertainty  * 

Pramod  P.  Khargonekar  t  and  Thomas  L.  Ting  § 


In  this  paper  we  describe  a  new  approach  for  de¬ 
tecting  faults  in  systems  in  the  presence  of  modeling  un¬ 
certainty.  Our  approach  is  interactive  and  relies  on  pro¬ 
cessing  true  time  domain  system  measurement  data  to 
determine  whether  or  not  the  system  is  operating  within 
an  expected  range  of  behaviors.  Our  results  are  appli¬ 
cable  to  either  open  or  closed  loop  systems  and  can  be 
implemented  in  a  numerically  efficient  fashion. 

INTRODUCTION 

As  today’s  state-of-the-art  control  systems  (i.e.  mil¬ 
itary,  aerospace,  chemical  processes  etc.)  become  increas¬ 
ingly  complex  the  problem  of  fault  detection  is  gaining  in 
importance.  For  many  of  these  systems  the  presence  of  a 
single  undetected  fault  can  lead  to  greatly  reduced  perfor¬ 
mance  or  worse  yet  a  catastrophic  failure.  Fault  detection 
algorithms  attempt  to  determine  when  a  system  is  oper¬ 
ating  outside  of  its  range  of  expected  behaviors.  Once  a 
fault  is  detected  various  means  exist  to  isolate  the  fault 
and  allow  the  system  to  operate  at  a  suboptimal  level. 
Some  control  designs  are  sophisticated  enough  to  achieve 
optimal  reconfiguration  in  the  presence  of  a  fault. 

Modeling  uncertainty  and  noise  inputs  both  compli¬ 
cate  the  problem  of  fault  detection.  For  example,  suppose 
a  control  engineer  possessed  a  perfectly  accurate  math¬ 
ematical  model  of  a  system  and  suppose  all  the  inputs 
could  be  measured.  Then  the  fault  detection  problem 
may  be  solved  through  the  following  basic  principle.  If 
the  measured  relationship  between  any  two  signals  within 
the  system  does  hot  agree  with  th^^^  relationship  predicted 
by  the  model  then  there  must  exist  a  fault  in  the  system. 


some  level  of  modeling  uncertainty.  Due  to  the  presence  of 
these  uncertainties,  it  is  clear  that  the  principle  described 
above  is  no  longer  directly  applicable  to  this  problem. 
In  this  case  the  job  facing  the  fault  detection  algorithm 
can  be  broken  into  two  steps.  First,  it  must  determine 
whether  a  discrepaincy  exists  between  the  measured  rela¬ 
tionship  between  any  two  signals  within  the  system  and 
the  expected  relationship  predicted  by  the  model.  Sec¬ 
ond,  if  a  discrepancy  does  exist  then  it  must  determine 
whether  this  information  is  truly  indicative  of  a  fault  ia 
the  system,  or  is  simply  the  result  of  noise  or  modeling  un¬ 
certainty.  Traditionally,  fault  detection  algorithms  have 
been  analyzed  in  a  probablilistic  setting  to  account  for 
random  noise  [2,3,4,6,10].  This  probabilistic  approach  is, 
however,  difficult  to  use  in  the  presence  of  unmodeled  dy¬ 
namics  which  is  typically  described  in  a  deterministic  set¬ 
ting.  Recently,  fault  detection  problems  in  the  presence 
of  unmodeled  dynamics  have  been  investigated  [1,7,8, 9]. 

In  this  paper  we  investigate  a  fault  detection  prob¬ 
lem  featuring  significant  modeling  uncertainty  due  to  un¬ 
modeled  dynamics.  A  typical  example  is  unmodeled  high 
frequency  dynamics.  We  develop  a  fault  detection  algo¬ 
rithm  that  accounts  for  these  unmodeled  dynamics.  Our 
approach  is  closely  related  to  the  recent  work  on  robust 
identification  as  well  as  model  validation  [5,7]. 

MATHEMATICAL  PRELIMINARIES 

There  exist  a  variety  of  mathematical  concepts  to 
characterize  the  time  and  frequency  domain  behavior  of 
plants  and  their  associated  input/output  signals.  In  this 
section  we  briefly  review  those  concepts  which  are  perti¬ 
nent  to  our  discussion. 

Signal  and  Operator  Norms 


Now  suppose  this  same  control  engineer  has  a  sys¬ 
tem  with  either  some  unmeasurable  noise  inputs  and/or 
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Let  £2”  be  the  set  of  aU  square  integrable  functions 
from  [0,  00)  to  Tv" 

=  {u(t)  :  f  {i)u(t)dt  <  00}.  (i) 

Jo 

For  all  signals  «(<)  in  this  set  we  may  define  the  £2  norm 


[|ti||2  =  u'^{t)u{t)dt. 


(2) 


This  norm  can  be  interpreted  as  the  “energy”  in  the  signal 
For  a  square  integrable  function  u  from  (0,  T)  to  72.'', 
define  the  partial  £2  norm  as 


This  norm  represents  the  “energy”  in  the  signal  u(<)  over 
the  time  interval  (0,T).  To  emphasize  the  most  recent 
signal  information  define  a  weighted  partial  £2  norm  as 


In  this  weighted  norm  all  the  past  values  of  u  are  weighted 
by  a  factor  which  decays  exponentially  as  a  function  of 
time.  This  limits  the  ability  of  accumulated  past  signal 
values  to  dilute  the  effects  of  new  information.  The  rate 
of  decay  is  controlled  by  the  selection  of  the  constant  <t. 

Modeling  Uncertainty 

Mathematical  models  are  an  attempt  to  analytically 
represent  the  behavior  of  a  true  physical  system.  Control 
designers  rely  heavily  on  such  models  in  designing  mod¬ 
ern,  high  performance  controllers.  In  reality,  however, 
true  physical  systems  are  never  perfectly  represented  by 
a  mathematical  model.  Thus,  it  is  important  for  a  con¬ 
trol  engineer  to  design  controllers  which  provide  not  only 
good  nominal  performance  but  are  also  robust,  i.e.  which 
perform  well  in  spite  of  plant  uncertainties. 

One  popular  approach  to  synthesizing  robust  con¬ 
trollers  is  to  build  uncertainty  into  the  plant  model.  In 
such  cases,  the  plant  is  not  represented  by  a  single  nom¬ 
inal  plant  model,  Po(s),  but  is  instead  allowed  to  be  any 
element  in  a  family  of  plant  models.  One  typical  family 
of  plant  models  is  given  by 

:r  =  {(/  +  AW(s))Po(s)  :  II Alice  <  6ma.}  (5) 

where  Po{s)  is  the  nominal  plant  model,  A  represents  an 
unknown,  linear,  time-invariant,  stable,  proper,  rational, 
norm-bounded  perturbation,  and  W(s)  is  a  frequency  de¬ 
pendent  weighting  function  which  characterizes  the  rel¬ 
ative  magnitude  of  modeling  uncertainty  at  various  fre¬ 
quencies.  Typically,  W(s)  takes  on  larger  values  at  higher 
frequencies  reflecting  the  inability  to  model  accurately  at 
these  frequencies.  Here  the  bound,  on  A  is  the  £2- 

induced  operator  norm  which  represents  the  maximum 
amount  of  modeling  uncertainty  associated  with  T . 

A  KEY  BOUND 

In  this  section  we  describe  a  key  time-domain  bound 
which  forms  the  basis  of  our  fault  detection  algorithm. 
Consider  the  generic  system  shown  in  Figure  1.  Here,  the 
input  signal  u  and  the  output  signal  y  are  related  by  the 
transfer  function  A(s)  through  the  expression  y  =  A{s)u. 

Suppose  that  the  magnitude  portion  of  the  fre¬ 
quency  response  of  A(s)  has  a  frequency  weighted  bound. 


In  other  words,  suppose  there  exists  a  nonnegative  real 
function  ^(a;)  such  that  ||A(jw)||oo  <  for  all  frequen¬ 
cies  w.  Moreover,  suppose  that  ^(a;)  is  such  that  there 
exists  a  rational,  minimum  phase,  proper,  stable  func¬ 
tion  W(3)  such  that  \W[juj)\  =  5(a;)  for  all  frequencies 
w.  Then  the  system  shown  in  Figure  1  can  be  redrawn  as 
shown  in  Figure  2  with  A  =  KW  where  l|A||oo  <  1. 

Since  ||  A  l|oo<  1  then  by  definition 

II  y  lir''’<ll  V 11^'*^  (6) 

for  any  arbitrary  time  t.  This  inequality  can  be  inter¬ 
preted  as  follows.  For  any  time  t  the  energy  of  the  output 
of  A  over  the  interval  (0,  t)  is  smaller  than  the  energy  of 
the  input  of  A  over  this  same  interval. 

More  generally,  suppose  two  signals  v  and  y  are  re¬ 
lated  by  a  causal  operator  A  such  that  11A||  <  1  where 
the  norm  on  A  is  the  £2 induced  norm.  Then  it  is  easy  to 
see  that 

II  y  Il2^“’'^<ll  V  Ilf  _  (7) 

holds.  Indeed,  it  has  been  shown  in  [7]  that  if  A  is  zillowed 
to  be  linear  time-varying,  then  this  is  a  tight  bound. 

In  real  time  fault  detection  algorithms,  it  is  desirable 
to  discount  past  information.  For  instance,  suppbse  A (5) 
is  a  linear,  time-invariant,  proper,  rational,  stable  func¬ 
tion  with  no  poles  in  i2e(s)  >  —  o’.  Moreover,  suppose 

^^max(A(s))  <  1.  (8) 

Then  it  is  possible  to  show  using  the  weighted  partial 
norm  that 

l|y(r)||f''^<||r;(r)||^';''>  (9) 

for  any  arbitrary  time  t. 

To  facilitate  implementation  of  either  Equation  (??) 
or  (??)  in  real-time  fault  detection  algorithms,  it  is  im¬ 
perative  to  minimize  computational  and  memory  storage 
requirements  associated  with  finding  the  upper  bounds. 
For  Equation  (??)  this  is  accomplished  by  defining 

=11  «'(r)  Ilf  ■‘^=  f  e<'-^K{Tfv{r)dr.  (10) 

J  O 

Here  1/(t)  is  a  monotonically  increasing  function  of  t  and 
its  derivative  is  given  by 

^(r)  = -<TV'(r)  + v(r)’'w(r).  (11) 

Now  V(t-i-At)  can  be  updated  recursively  by  nu¬ 
merically  integrating  Equation  (??).  This  computation 
requires  only  the  most  recent  measurement  value  v(t),  the 
most  recent  computed  veJue  of  K(<),  the  timestep  At,  and 
the  constant  a.  In  addition  to  computational  simplicity 
this  approach  reduces  memory  requirements  by  eliminat¬ 
ing  the  need  to  store  all  values  of  past  measurements  from 
time  zero  to  the  present.  The  upper  bound  for  Equation 
(??)  can  be  obtained  by  following  a  similar  procedure. 


FAULT  DETECTION  ALGORITHM 


In  this  section  we  use  the  key  bound  described  in  the 
previous  section  to  develop  our  fault  detection  algorithm. 
We  first  describe  the  general  ideas  behind  our  algorithm 
and  outline  their  implementation.  Next,  we  address  more 
specific  algorithm  implementation  issues  such  as  select¬ 
ing  an  appropriate  fault  detection  threshold  to  account 
for  noise,  modeling  uncertainty  and  the  tradeoff  between 
various  error  probabilities. 

Algorithm  Outline 

We  describe  our  algorithm  in  the  context  of  a  typi¬ 
cal  open  loop  system  , as  shown  in  Figure  3.  This  system 
contains  a  plant  P,  featuring  multiplicative  modeling  un¬ 
certainty,  which  is  allowed  to  be  any  member  of  a  family 
of  plants  T  described  by  Equation  (??).  For  simplicity 
we  selected  a  weighting  function  W{3)  such  that  Smax  is 
normalized  to  one.  In  addition  to  modeling  uncertainty, 
the  plant  P  also  features  an  zulditive  failure  input  signal 
/.  This  signal  is  zero  under  normal  plant  operations  and 
is  nonzero  when  the  plant  is  operating  in  a  failed  state. 
Each  possible  failure  of  the  plant  is  associated  with  a  dif¬ 
ferent  failure  signal  /.  For  example,  if  a  system  is  subject 
to  three  different  types  of  failures,  then  there  would  be 
three  different  failure  input  signals  /i,/?,  and  /a.  The 
characteristics  of  each  /  are  intended  to  replicate  the  im¬ 
pact  an  associated  failure  would  have  on  the  plant.  It  is 
assumed  that  the  plant  operator  has  complete  knowledge 
of  all  possible  failure  signals,  but  does  not  know  which,  if 
any,  signals  are  present  at  any  given  time. 

The  open  loop  system  features  a  plant  output  mea¬ 
surement  y  corrupted  by  additive  measurement  noise  n. 
Although  individual  values  of  n  are  unknown,  we  assume 
that  certain  properties  of  n  are  known.  For  instance,  n 
may  be  represented  by  a  stochastic  noise  model  such  as 
a  Gaussian  distribution  with  zero  mean  and  a  fixed  stan¬ 
dard  deviation.  Alternatively,  n  could  be  represented  by 
a  deterministic  noise  model.  Here,  n  would  not  be  de¬ 
scribed  through  probability  distributions  but  vyould  in¬ 
stead  be  restricted  by  deterministic  constraints,  such  as 
weighted  £2  or  £00  norms. 

The  central  idea  of  our  fault  detection  algorithm  is 
to  use  available  signad  measurements  to  analytically  re¬ 
construct  the  input  and  output  signals  to  the -^uncertainty 
block  A.  This  signal  set  is  then  examined  using  the  key 
bound  from  Equation  (??)  to  determine  whether  or  not 
it  complies  with  the  apriori  modeling  assumptions  on  the 
size  of  A.  In  particular,  we  check  to  ensure  that  the  mea¬ 
surement  data  could  indeed  have  been  generated  by  a  per¬ 
turbation  of  size  Smax  or  less.  With  Smax  normalized  to 
1  our  fault  detection  algorithm  basically  checks  whether 
or  not  the  output  signal,  dour,  of  A  has  more  or  less  en¬ 
ergy  than  the  input  signal  <i,„.  If  Smax  were  not  nor¬ 
malized  then  our  fault  detection  algorithm  could  simply 
check  whether  or  not  dour  has  more  or  less  energy  than 
Smax  times  d,ri. 


From  Figure  3,  d,n  =  V^PoT,  This  signal  is  con- 
struetable  because  the  reference  signal  r  and  the  trans¬ 
fer  functions  W(s)  and  Po{s)  are  all  known.  Constructing 
dour  is  slightly  more  complicated.  From  Figure  3  we  know 
that  the  plant  output  measurement  y  is  given  by 

V  =  Por  +  AWPor  +  /  +  n.  (12) 

Now  define  dour  =  AWPo^*-  Since  /  and  n  are  unknown 
we  approximate  dour  as 

dout  ^  y  —  Po^  =  dour  “h  /  +  ri.  (I^) 

Assume  that  the  noise  signal  n  is  zero.  Then  using 
the  signals  described  above  we  can  detect  the  presence  of 
a  fault  by  computing  whether 

II  y  -  PoT  -  II  WPoT  0  =  (14) 

If  this  inequality  holds  then  di„  has  more  energy  than  do«g 
in  the  time  interval  [0,  fj.  This  result  is  consistent  with 
an  uncertainty  block  A  of  norm  less  than  or  equal  to  one, 
and  thus  the  fault  detection  algorithm  concludes  that  no 
fault  is  present.  Conversely,  if  this  inequality  does  not 
hold  then  by  analogous  reasoning  this  result  is  inconsis¬ 
tent  with  our  modeling  assumptions  and  the  fault  detec¬ 
tion  algorithm  concludes  that  a  fault  must  be  present. 

Operationally,  this  fault  detection  algorithm  is  im¬ 
plemented  in  three  steps.  First,  any  desired  r  is  input  to 
the  true  physical  system  and  the  resulting  y  is  recorded. 
Second,  knowledge  about  r,  Po,  and  W,  is  used  to  ana¬ 
lytically  compute  PoT  and  WPoT.  Third,  the  two  norms 
in  Equation  (??)  are  obtained  recursively  by  numerically 
integrating  Equation  (??). 

A  convenient  method  of  checking  the  inequality  in 
Equation  (??)  is  to  display  the  difference 

II  y  -  PoT  -  II  wp,T  (15) 

graphically  as  a  function  of  time.  By  compelling  this  value 
versus  Lthresh  we  can  easily  see  whether  or  not  a  fault 
exists. 

Thus  far  we  have  discussed  our  fault  detection  adgo- 
rithm  only  in  the  context  of  open  loop  systems.  However, 
the  same  results  are  directly  applicable  to  closed  loop  sys¬ 
tems,  as  shown  in  Figure  4,  provided  that  the  control 
input  u  is  measurable.  If  this  condition  holds  then  the 
identical  algorithm  is  applicable  to  closed  loop  systems 
with  the  sole  modification  that  the  reference  signal  r  is 
replaced  by  the  control  input  w.  Thus,  for  the  closed  loop 
system  fault  detection  algorithm  the  input  and  output 
signals  associated  with  A  are  given  by  din  =  W PoU  and 
dout  =  AWPoU  ^  y  —  P^u.  All  other  aspects  of  the  algo¬ 
rithm  remain  unchanged.  For  the  remainder  of  this  paper 
we  will  continue  to  discuss  our  fault  detection  algorithm 
in  the  context  of  open  loop  systems  noting  that  all  results 
are  also  applicable  to  closed  loop  systems. 

In  applications  we  recommend  using  the  weighted 
partial  £2“norm  instead  of  the  standard  partial  £2-norm. 
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This  substitution  is  allowable  because  for  any  plant  P  in 
the  family  described  by  Equation  (??),  A  is  a  stable 
perturbation  which  implies  that  there  exists  some  positive 
<T  such  that  A(3)  has  no  poles  in  Re{3)  >  — <7. 

Without  the  inclusion  of  the  exponential  forgetting 
factor  in  the  partial  £2-norm,  it  is  possible  that  a  long 
initial  period  of  no  fault  performance  could  dilute  the 
ability  of  a  fault  associated  transient  to  suddenly  alter 
the  difference  ||  y  —  PoT  ~  ||  W Por  .  With 

the  exponential  forgetting  factor  present,  the  most  recent 
data  values  are  emphasized  and  this  risk  is  lessened. 

Threshold  Selection 

In  reality,  the  threshold,  Lthrcsh  (currently  zero), 
on  the  right  hand  side  of  Equation  (??),  is  too  stringent 
for  practical  applications.  This  value  was  derived  under 
the  assumption  that  n  is  zero.  Clearly,  the  presence  of  a 
nonzero  n  will  alter  the  energy  in  y,  and  hence  the  en¬ 
ergy  of  the  computed  dout,  commensurate  with  the  noise 
chauracteristics.  To  compensate,  Lthrcsh  must  be  raised, 
with  its  ultimate  selection  dependent  on  the  specific  per¬ 
formance  requirements  of  the  fault  detection  algorithm. 

Fault  detection  algorithms  are  subject  to  two  types 
of  errors:  a  False  Alarm  or  a  Missed  Fault.  A  False 
Alarm  (FA)  is  an  error  which  occurs  when  there  is  no 
fault  present  in  the  system  but  the  fault  detection  algo¬ 
rithm  mistakenly  signals  the  presence  of  a  fault.  A  Missed 
Fault  (MF)  is  an  error  which  occurs  when  there  is  a  fault 
present  in  the  system  but  it  is  not  detected  by  the  fault 
detection  algorithm.  In  our  case,  as  is  true  with  almost 
all  fault  detection  algorithms,  the  probability  of  either  of 
these  errors  is  directly  related  to  the  value  of  Lthrcsh-  We 
would  ideally  like  to  simultaneously  minimize  both  the 
probability  of  a  MF,  (p(MF)),  and  the  probability  of  a 
FA,  (p(FA)).  However,  such  a  minimization  is  impossi¬ 
ble  because  as  the  vzdue  of  Lthrcsh  varies,  there  exists  an 
inherent  tr^ideoff  between  p(MF)  and  p(FA). 

No  standard  procedures  exist  to  compare  the  rela¬ 
tive  importance  of  MF’s  versus  FA’s.  In  fact,  their  rel¬ 
ative  importance  must  be  individually  assessed  for  each 
application.  One  can  easily  envision  different  scenarios 
where  MF’s  range  from  being  critically  important  to  op¬ 
erationally  acceptable.  Indeed,  unless  a  MF  were  truly 
critical  we  certainly  would  not  want  to  set  Lthrcsh  so  low 
that  we  are  continually  shutting  down  the  system  in  re¬ 
sponse  to  a  slew  of  FA  errors. 

Regardless  of  the  choice  of  Lthrcsh  our  fault  detec¬ 
tion  algorithm  will  be  unable  to  discern  the  difference  be¬ 
tween  /  and  n.  Thus,  to  completely  eliminate  the  possibil¬ 
ity  of  FA’s  our  fault  detection  algorithm  requires  setting 
Lthrt.h  =  2||n||2ff||«f,„||2»  +  l|n||L-  This  value  is  obtained 
by  computing  the  maximum  energy  difference  achievable 
between  dout  and  din  given  a  noise  input  of  energy  ||n||L- 

Conversely,  completely  eliminating  the  possibility  of 
MF’s  requires  setting  Lthrcsh  to  a  negative  number.  From 


Equation  (14)  such  a  value  is  intuitively  unsettling  be¬ 
cause  it  requires  that  dout  have  a  prespecified  amount  of 
energy  less  than  din  in  order  for  a  fault  not  to  be  declared. 

In  reality  the  actual  value  of  Lthrcsh  will  lie  between 
these  two  extremes.  For  an  arbitrary  value  of  Lthrcsh^  we 
would  like  to  compute  the  minimum  size  of  a  fault  /  to 
assure  detection  by  our  algorithm.  After  some  algebraic 
computations  we  find  that  all  faults  /  such  that 

ll/Ihtr  >  ||rt||2o- +  ll<io«t||2<r  +  \/T  thresh  +  \\din\\l^-  (16) 

are  detectable  by  our  fault  detection  algorithm.  From  this 
equation  it  is  clear  that  there  are  three  key  faurtors  influ¬ 
encing  the  detectability  of  various  faults:  1)  the  energy 
levels  of  the  signals  din  ajid  dout,  2)  the  energy  levels  of 
/  and  n  and  3)  the  value  of  the  threshold  Lthrcsh- 

We  can  interpret  the  results  of  Equation  (16) 
through  a  simple  example.  Suppose  we  have  a  case  where 
both  ||di„|(  and  ||doue||  are  zero  in  steady  state.  Also,  sup¬ 
pose  the  two  signals  /  and  n  have  the  same  energy  levels 
and  Lthrcsh  is  any  nonzero  number.  Then  it  is  conceiv¬ 
able  (although  unlikely)  that  n  could  completely  cancel 
the  signal  /  and  it  would  appear  as  if  no  fault  is  present. 
In  fact,  to  mask  the  presence  of  an  existing  fault,  n  need 
not  completely  cancel  /,  but  it  must  only  make  it  appear 
as  if  the  combined  signal  (n  -h  /)  has  less  energy  than 
Lthrcsh-  Therefore,  to  guarantee  that  a  failure  can  be 
detected,  the  associated  failure  signal  /  must  satisfy 

ll/ljzer  ^  +  ^Lthrcsh-  (l*^) 

From  this  equation  it  is  clear  that  as  either  the  energy 
level  of  n  rises  or  Lthrcsh  rises  the  energy  requirements 
of  /  to  assure  detection  increases.  This  does  not  mean 
that  failure  signals  with  less  energy  will  not  be  detected. 
However,  it  does  mean  that  the  probability  of  detecting  a 
fault  associated  with  a  fixed  energy  failure  signed  decreases 
as  either  Lthrcsh  or  the  energy  in  n  increases. 

AN  EXAMPLE 

We  now  demonstrate  the  operation  of  our  fault  de¬ 
tection  algorithm  through  an  illustrative  example.  This 
example  is  based  on  the  open  loop  system  model  shown 
in  Figure  3.  The  plant  model  is  represented  by  a  family 
of  plants  as  in  Equation  (??)  with  a  nominal  plant  model 
given  by  Po(s)  =  7^  and  uncertainty  bound  Smaz  =  1. 
The  uncertainty  block  A  consists  of  three  high  frequency 
second  order  flex  modes  and  is  given  by 

^  200  200  200 

+  15s  -h  400  lOs  -f-  1600  +  10s  -h  4900  ‘ 

(18) 

A  frequency  magnitude  response  plot  of  A  is  shown  in  Fig¬ 
ure  5.  Note  that  the  magnitude  response  of  A  is  below 
unity  for  ail  frequencies  so  ||A||  is  indeed  less  than  or  equal 
to  6maz  =  1-  The  modeling  uncertainty  weighting  func¬ 
tion  is  given  by  W(s)  =  .  This  function  was  chosen 

to  emphasize  the  modeling  errors  at  high  frequencies  while 


minimizing  the  errors  at  lower  frequencies.  The  plant  out¬ 
put  measurement  y  is  corrupted  by  an  unknown  noise  n, 
and  for  our  example  we  assume  that  ||n||2  <  .0025. 

For  our  fault  detection  algorithm  we  set  Lthrcsh  = 
.5||n||2<r|ldin||2<r  +  H^llzo--  This  is  an  intermediate  value  of 
Lthrcsh  which  does  not  preclude  the  possibility  of  MF’s, 
but  is  also  not  extremely  sensitive  to  FA’s.  Note  also  that 
this  is  a  time-varying  threshold  which  is  dependent  upon 
the  computed  values  of  d,n.  We  ran  a  variety  of  time 
domain  simulations  using  a  reference  step  input  r  of  mag¬ 
nitude  1.1.  We  used  the  data  for  r  and  y  to  construct 
din  and  dout-  Finally,  the  weighted  parti2J  £2-norms  (en¬ 
ergies),  Ein  and  Eout,  of  each  of  these  two  signals  were 
computed  using  Equations  (??)  and  (??). 

In  the  first  simulation  we  ran  the  system  with  no 
faults  present  and  obtained  the  time  histories  shown  in 
Figure  6.  dm  (solid  line)  and  dout  (dotted  line)  are  shown 
in  Figure  6a  while  Ein  (solid  line)  and  Eout  (dotted  line) 
are  shown  in  Figure  6b.  Note  that  as  dm  reaches  its  steady 
state  value  about  zero,  the  initial  bump  in  Ein  also  decays 
to  zero.  This  is  explicitly  due  to  the  exponential  forget¬ 
ting  factor  built  into  V{i).  Figure  6c  displays  the  weighted 
partial  energy  of  n.  In  a  real  system  this  value  would  not 
be  available,  but  in  our  simulation  we  measured  this  value 
to  ensure  that  our  noise  inputs  adhered  to  our  assump¬ 
tions.  Finally,  the  difference  Eout  —  Ein  (solid  line)  and 
the  threshold  Lthrcsh  (dotted  line)  are  shown  in  Figure 
6d.  In  this  particular  case  the  energy  difference  is  always 
below  LthrcMh  so  the  fault  detection  algorithm  correctly 
concludes  that  no  fault  is  present. 

In  the  second  simulation  we  configured  the  system 
such  that  a  “large”  failure  corresponding  to  a  step  input  of 
.11  would  occur  at  time  t  =  2  seconds.  The  corresponding 
results,  presented  in  an  analagous  manner  to  Figure  6, 
are  shown  in  Figure  7.  Note  that  the  presence  of  the 
failure  signal  /  is  clearly  evident  in  dout  and  Eout  for  times 
greater  than  2  seconds.  In  this  case  the  energy  difference 
shown  in  Figure  7d  exceeds  Lthre»h  almost  immediately 
after  the  fault  has  occured  (at  roughly  t  =  2.35  seconds) 
so  the  fault  detection  algorithm  very  quickly  and  correctly 
concludes  that  a  fault  is  present. 

In  the  third  time  domain  simulation  we  configured 
the  system  such  that  a  “small”  failure  corresponding  to  a 
step  input  of  .03  would  occur  at  time  1  =  2  seconds.  The 
corresponding  results  are  shown  in  Figure  6,  In  this  case 
the  deviations  of  dout  and  Eout  from  the  no  fault  present 
case  (Figure  6)  are  not  as  pronounced  as.  in  the  “large” 
failure  case  (Figure  7).  From  the  plots  shown  in  Figure 
8d  we  can  see  that  it  takes  roughly  three  seconds  after  the 
fault  occurs  before  the  energy  difference  exceeds  Lthrcsh^ 
Thus,  the  fault  detection  algorithm  correctly  identifies  the 
presence  of  this  smaller  fault  but  not  as  quickly  as  it  iden¬ 
tified  the  larger  fault. 

CONCLUSIONS  AND  RECOMMENDATIONS 

In  this  paper  we  have  described  a  new  time-domain 
based  algorithm  for  detecting  faults  in  systems  in  the  pres¬ 


ence  of  modeling  uncertainty.  Our  algorithm  relies  on 
processing  true  time  domain  system  measurement  data 
to  analytically  reconstruct  key  signals  which  characterize 
the  modelling  uncertainty.  These  signals  are  compared 
with  apriori  assumptions  about  the  plant  model  to  deter¬ 
mine  whether  or  not  a  fault  is  present.  The  algorithm 
b  easy  to  implement  on  either  an  open  or  closed  loop 
system  and  its  use  was  demonstrated  via  a  simple  ex¬ 
ample.  Several  critical  implementation  issues  were  also 
addressed.  An  efficient  numerical  implementation  of  the 
algorithm  was  presented  which  featured  reduced  memory 
and  computational  requirements.  In  addition,  a  weighted 
partial  energy  criteria  was  employed  which  ensures  that 
the  algorithm  emphasizes  the  newest  data. 

Certain  crucial  issues  remain  to  be  addressed.  These 
include  1)  optimal  threshold  selection  strategies  and  the 
resulting  tradeoff  between  Fabe  Alarm  and  Missed  Fault 
error  probabilities  and  2)  detecting  and  dbcerning  a  single 
failure  from  among  a  set  of  possible  failures.  These  issues 
are  currently  under  investigation. 

REFERENCES 

1.  Emami-Naeini,  A.,  M.  Akhter,  and  S.  Rock, 
“Effect  of  Model  Uncertainty  on  Failure  Detection:  The 
Threshold  Selector,”  IEEE  Transactions  on  Automatic 
Control,  Vol.  33,  No.  12,  pp.  1106-1115,  1988. 

2.  Friediand,  B.,  “Maximum  Likelihood  Failure  De¬ 
tection  of  Aircraft  Flight  Control  Sensors,”  Journal  of 
Guidance,  Control  and  Dynamics,  Sept.-Oct.  1982,  pp. 
498-503. 

3.  Isermann,  R.,  “Process  Fault  Detection  Based  on 
Modeling  and  Estimation  Methods-A  Survey”,  Automat- 
ica,  Vol.  20,  No.  4,  pp.  387-404,  1984. 

4.  Kerr,  T.,  “Decentralized  Filtering  and  Redun¬ 
dancy  Management  for  Multisensor  Navigation”,  IEEE 
Transactions  on  Aerospace  and  Electronic  Systems,  Vol. 
23,  No.  1,  pp.  83-119,  1987. 

5.  Krause,  J.  and  P.  Khargonekar,  “Parameter  Iden¬ 
tification  in  the  Presence  of  Non- Parametric  Dynamic  Un¬ 
certainty”,  Automatica,  Vol.  26,  pp.  113-124,  Jan.  1990. 

6.  Patton,  R.,  P.  Frank  and  R.  Clark  (Eds.),  “Fault 
Diagnosis  in  Dynamic  Systems,  Theory  and  Application”, 
Prentice  Hall,  1989. 

7.  Poolla,  K.,  P.  Khargonekar,  A.  Tikku,  J.  Krause, 
and  K.  Nagpal,  “A  Time-  Domain  Appraoch  to  Model 
Validation”,  to  appear  in  IEEE  Transactions  on  Auto¬ 
matic  Control. 

8.  Smith,  R.  and  J.  Doyle,  “Model  Validation:  A 
Connection  Between  Robust  Control  and  Identification”, 
IEEE  Transactions  on  Automatic  Control,  Vol.  37,  No. 
7,  pp.  942-952,  1992. 

9.  Smith,  R.,  “Model  Validation  and  Identification 
for  Systems  in  Hqo  and  /i”,  Proceedings  of  the  American 
Control  Conference,  pp.  2852-2856,  June  1992. 


10.  Willsky,  A.,  “A  Survey  of  Design  Methods  for 
Failure  Detection  in  Dynamic  Systems”,  Automatica,  Vol. 
1976,  pp.  601-611. 


To:  Pat  Overstreet,  Honeywell  SRC,  (612)951-7041 
cc:  Harry  Kirschke,  Honeywell  SASSO,  (813)539-5384 
Charlie  Poe,  Honeywell  SASSO,  (408)756-2781 
Bill  Pouts,  Honeywell  SRC,  (612)951-7034 
John  Weyrauch,  Honeywell  SRC,  (612)951-7280 
Jim  Krause,  Honeywell  SRC,  (612)951-7292 
Lew  Crouder,  Lockheed  Missiles  and  Space  Co.,  (408)756-2781 
BUI  EdwardSj  LcK:kheed  Missiles  and  Space  Co.,  C408)756-2781 

From;  Mike  Elgersma,  Honeywell  SRC,  (612)951-7208 
Date:  June  01,  1993 

Subject:  Final  Report  K)  Honeywell  SASSO  for  Lockheed  THAADS  work 

Contract  Name:  THAAD/IAP  Support 
Purchase  Order:  23070298 
Contract/Project  Number;  FI  183 


Enclosed  is  the  final  report  for  work  done  by  Honeywell  SRC  on  THAAD. 


Michael  R.  Elgersma 


0 


Date;  June  01,  1993 


Final  Report  for  THAAD/IAP  Support  Executive  Summary 

This  final  report  covers  work  done  on  May  18-21,  1993  to  provide  an  independent  analysis  of 
a  flexible  missile  control  problem.  During  this  time,  I  went  to  Lockheed  to  become  familiar 
with  an  autopilot  stability  issue  caused  by  flexibility  of  a  thin  missile.  I  evaluated  the  prob¬ 
lem  and  concluded  that  the  tail  mounted  gyro  reduced  the  response  of  the  first  flex  mode  by  a 
factor  of  ten.  The  extra  gyro  reduces  a  very  significant  flex  problem  to  a  marginal  problem. 
The  remainder  of  the  problem  can  be  handled  by  some  combination  of  flex  filtering,  passive 
damping,  and  employing  aero  devices  to  move  the  c.p.  back  during  the  high  dynamic  pres¬ 
sure  portion  of  flight. 

The  following  section  gives  several  formulas  for  the  unstable  aero  mode,  the  size  and  location 
of  the  first  flex  mode,  as  well  as  an  envelope  for  the  remaining  flex  modes.  These  formulas 
are  useful  for  determining  the  fundamental  physical  tradeoffs  in  designing  a  uniform  cylindri- 
cally  shaped  missile.  I  would  like  to  thank  Bill  Edwards,  Robert  Felder,  Conrad  Woo,  John 
Sesak,  and  Doug  Discher  for  there  help  in  providing  me  the  necessary  information  for  this 
project. 
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Final  report  for  THAAD/IAP  Technical  section: 


Summary 


The  most  important  parameters  in  the  control  of  an  unstable  flexible  missile  are: 


tOflex. 


=  the  ratio  of  the  unstable  aero  fiiequency  and  the  first  flex  mode  frequency, 


(0, 


'acro 


®^«ctnator 

AH' 


=  the  ratio  of  the  unstable  aero  frequency  and  the  actuator  bandwidth. 


=  uncertainty  in  the  flex  mode  shapes. 


^  =  the  flex  damping  ratio, 

Cc  =  the  controller  damping  ratio, 

and  the  size  of  the  resulting  flex  resonance  peaks  in  the  Bode  plots. 


With  only  one  gyro  mounted  at  the  nose 


Bode  Plot  peak  = 
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With  blended  fore  and  aft  gyros. 


Bode  Plot  peak  = 
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l»A^I(2) 


where 

L  =  the  length  of  the  missile 
R  =  the  radius  of  the  missile 
t  =  the  skin  thickness  of  the  missile 

р, n  =  the  density  of  the  entire  missile 

с. p.  =  the  center  of  pressure  of  the  missile 
c.g.  =  the  center  of  gravity  of  the  missile 

=  the  aerodynamic  normal  coefficient  =  2 
Vj  =  flex  boundary  condition  coefficient  =  22 
E  =  Young’s  Modulus  of  the  missile  skin 
q  =  dynamic  pressure 

C  the  flex  damping  ratio  (between  .01  and  .05) 

Cc  =  the  controller  damping  ratio  (between  .2  and  1) 
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Flex  Dynamics 


The  geometry  of  the  THAAD  missile  is  closely  approximated  as  a  free-free  uniform  thin 
annulus  of  length  L,  radius  R,  skin  thickness  t  (with  t  «  R  «  L),  density  p„„  and  Young’s 
modulus  E.  Only  the  metal  skin  (of  thickness  t  «  R)  contributes  to  the  stiffness,  while  the 
entire  cylinder  (of  thickness  R)  contributes  to  the  density.  As  the  fuel  bums  (uniformly  along 
the  length  of  the  missile),  decreases. 

The  uniformity  of  the  missile  allows  the  use  of  a  simple  analytic  formula  to  approximate  the 
frequency  of  the  first  few  flex  modes,  The  following  formula  is  taken  from  W.  C. 

Young,  "Roark’s  Stress  and  Strain,"  Sixth  Edition,  McGraw-Hill,  1989. 


(3) 


where 


Vi  =  22.4,  V2  =  61.7,  V3  =  121,  V4  =  200,  and  V5  =  299. 


The  damping  ratio,  is  approximately  .01  since  there  are  almost  no  joints  in  the  structure. 

The  odd  mode  shapes  have  the  same  displacement  but  opposite  slope  at  the  nose  and  tail. 
The  even  mode  shapes  have  opposite  displacement  but  same  slope  at  the  nose  and  tail. 

Let  Tj.  be  the  control  torque  applied  at  the  tail  (x=0)  of  the  missile  using  thrust  vector  control 

(Tc  =  —  Thrast  sin(5)).  Let  'Fi(x)  be  the  m^o^sI6pe~of  mode  i  at  the'i^nt  x  (x=0  at  tail,  x=L 

at  nose).  The  mode  slope  gives  the  coupling  between  torque  inputs  and  the  flex  response,  as 
well  as  from  the  flex  response  to  the  angular  rate  measurements.  The  flexible  body  transfer 
function  from  control  torque  to  pitch  rate  (at  the  point  x)  can  be  approximated  as: 


e(x) 

'Cc 


4> 


Ti(x)  s  'Pi(0) 


i  s^  +  2C(0fl«iS  + 


(4) 


Since  the  missile  is  a  uniform  shape,  all  the  mass  participates  in  each  mode,  therefore  'FKx)  is 
about  size  1,  i.e. 
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where  AT  represents  the  uncertainty  in  the  match  between  the  mode  slope  at  the  fore  and  aft 
locations.  Since  the  missile  is  nearly  uniform  in  shape,  the  slopes  should  match  to  within 

AVP 

10%,  i.e.  I-^^I  <  .1  for  this  missile. 


Rigid  Body  Dynamics  due  to  Aero 

The  aero  torque  due  to  flying  through  air  of  density  p  at  speed  V  is  approximately  given  by: 

'c.ero  =  (‘ApV^)(7tR^)(C.p.  -  C.g.)  Cn,  (©-“JO  (6) 


We  will  assume  that  the  flight  path  angle  y  changes  much  slower  than  the  vehicle  pitch  angle 
0,  and  will  linearize  about  0  =  ©  -  y.  Then  the  rigid  body  pitch  dynamics  can  be  approxi¬ 
mated  with  the  following  second  order  system: 


J  0  =  CApV2)(jdl2)(c.p.  -  c.g.)  Cn.  e 


C7) 


If  we  rewrite  this  as: 


then  the  unstable  (c.p.  >  c.g.)  aero  frequency  is  approximately  given  by: 


to. 


C/ipV2)(rtR2)(c.p.-c.g.)Cv 


(8) 


The  moment  of  inertia  of  a  uniform  thin  (R  «  L)  cylinder  of  length  L,  radius  R,  and  density 
Pro  is  approximately  given  by: 
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(10) 


which  gives 


co„ 


12(>^pV")(c.p.  -  c.g.)  Cn„ 
__ 


The  rigid  body  transfer  function  from  control  torque  to  angular  rate  is: 


Combined  Flex  and  Rigid  Dynamics 


The  ratio  of  equations  1 1  and  3  gives: 


tOaefo 


(13) 


The  combined  rigid  and  flexible  body  transfer  function  from  control  torque  to  pitch  rate  (at 
the  point  x)  can  be  approximated  as: 


e(x) 

% 


s 


TjCx)  S  TjCO) 

+  2Cc0flex;S  +  a)fl„2 


(14) 


Actuators 

The  THAAD  actuator  frequency  is  currently  between  the  first  and  second  flex  modes. 

^  ^actuator  <  (15) 

The  transfer  function  given  in  equation  14  must  be  modified  at  frequencies  above  the  actuator 
frequency. 


Controller 

Assume  there  are  rate  gyros  at  locations  x^.  and  attitude  gyros  at  locations  x^g^^.  In  order  to 
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robustly  control  the  unstable  aero  rigid  body  mode,  the  controller  must  have  approximately 
twice  as  much  torque  as  the  unstable  aero  torque. 

©/  =  (16) 
A  simple  PD  controller  (with  damping  ratio  .2  <  Cc  <  1)  would  then  have  the  form 


tc  =  -J 


2Cc®c  ZfCj  +  coc^  i;[dk  0(x,g^] 

j  k 


(17) 


where  the  gyros  have  been  blended  to  give  the  correct  rigid  body  information: 

ECj=l 

j 

Zdk=l 

k 


At  high  frequency  (ie  (o  >  tOc)  the  rate  term  dominates  the  position  term,  so  at  high  frequency: 


Xc  =  -J(2Cc®c)B0(V^ 

j 


(18) 


combining  equations  14  and  18  gives  the  loop  gain  of  the  combined  controller,  K(s),  and 
plant,  G(s): 


A 


K(s)G(s)  = 


2CcC0c  S  ^  2Cc(0c  S  %(P) 

-  tOaero^  i  j  +  2Cc0fl„.S  + 


(19) 


Fore  and  Aft  Rate  Gyros 

If  only  one  gyro  is  used,  it  must  be  placed  in  the  nose,  since  the  rest  of  the  vehicle  separates 
after  burnout.  For  controlling  the  flexible  booster,  it  would  be  better  to  have  the  rate  gyro 
mounted  at  the  same  location  as  the  actuator  (at  the  tail)  since  then  the  measurement  and  the 
applied  torque  would  be  collocated  and  would  remain  in  phase  for  all  flex  modes.  This 
allows  the  controller  to  remove  energy  by  making  the  torque  proportional  to  the  sensed  angu¬ 
lar  rate.  This  would  ensure  that  all  flex  modes  were  phase  stable,  except  for  the  fact  that  the 
actuator  dynamics  ruin  this  argument  above  the  actuator  frequency.  Consequently,  it  is  safer 
to  gain  stabilize  the  flex  modes. 
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By  blending  the  signals  from  a  nose  and  tail  gyro,  the  odd  flex  modes  will  nearly  cancel, 
since  they  have  opposite  mode  slopes  at  the  fore  and  aft  locations.  This  not  only  stabilizes 
the  odd  flex  modes,  but  also  reduces  the  ringing  that  would  be  present  if  we  relied  only  on  a 
single  collocated  gryo. 


If  one  rate  gyro  is  placed  at  the  nose,  ( x^^  =  L ),  and  the  second  rate  gyro  is  placed  at  the 
tail,  ( x^j  =  0 ),  then 


Ti(0)  =  T'iCL)  Ti(0)  =  (-1)‘  (1  + 


'F 


and 


'i'i(xrg;'i'i(0)  =  'i'i(0)^i(0)=l 


Since  the  missile  is  very  uniform,  the  mode  slopes  are  known  to  within  10%, 

1^1  <■> 


The  loop  gain  with  a  fore  and  aft  gyro  is  given  by: 


(20) 


-K(s)G(s)  = 


2  E 


S  -  ©; 


aero  i 


S^  +  2C®flex,S  + 


with  Cl  +  C2  =  1. 


Evaluating  the  above  expression  at  the  flex  frequencies  gives  the  size  of  the  peaks: 


(-i)"cia  +  -^)  +C2 


Cwfl, 


(-1)"  Cld  +  ^)  +  C2 


CtOfl, 


If  Cl  =  0  and  C2  =  1,  then  the  size  of  the  flex  peaks  are: 

Cc^CO. 


iKaCOfleOGOCOflex,)!  = 


(22) 


(23) 


(24) 
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(25) 


If  Cl  =  .5  =  C2,  then  the  size  of  the  odd  flex  peaks  are: 


lK(jo)fl.x„)GOcOfl,Ol  = 


A'F,  Cc^O>a 


=V 


^  CtOflex, 


Combining  equation  13  with  equations  24  and  25  gives  equations  1  and  2  in  the  summary. 


Recall  that  we  are  assuming  that  the  controller  rate  term  dominates  the  controller  attitude  term 

®flex, 

by  a  factor  of - .  The  blending  is  done  only  on  the  rate  gyros,  so  if  the  loop  gain  in 


oa 


equation  25  drops  below  the  attitude  loop  gain.  i.e.  i 


A'F,  .  “aero 


'J'  “flex. 

ing  through  the  unblended  attitude  loop  may  become  larger  than  the  rate  loop. 


,  then  the  flex  signals  com- 


Detuning  the  Blended  Rate  Gyros 

Depending  on  whether  A'F  is  positive  or  negative,  setting  c,  =  .5  =  C2  could  result  in  either  the 
fore  or  aft  gyro  dominating.  In  order  to  design  a  flex  filter  to  further  reduce  the  first  flex 
peak,  it  is  necessary  to  know  the  phase  of  the  signal,  therefore  it  is  necessary  to  know  which 
gyro  is  dominating.  It  may  be  necessary  to  set  Ci  =  .45  and  C2  =  35  to  ensure  that  the  aft  gyro 
dominates.  This  will  slightly  increase  the  size  of  the  first  flex  peak,  but  will  ensure  proper 
phasing  of  the  blended  gyro  signals  when  there  is  10%  uncertainty  in  the  mode  slopes. 


Flex  Filter  Design  Suggestions 

The  flex  filters  should  add  as  little  phase  lag  as  possible.  If  the  gain  is  to  be  rolled  off  uni¬ 
formly  above  some  frequency,  then  a  Butterworth  filter  should  be  used  to  minimize  phase 
loss.  If  the  flex  frequencies  (which  increase  as  fuel  is  burned)  are  known  well  enough,  then 
the  flex  modes  that  are  not  phase  stable  can  be  notched  out.  A  narrow  notch  filter  introduces 
less  phase  lag  than  a  broadband  filter. 

Analyzing  Actuator  Nonlinearities 

The  actuator  nonlinearity  (due  to  the  backlash,  hysteresis,  etc.  of  the  EMA  gearing)  can  be 
analyzed  using  describing  function  analysis.  If  the  gear-drive  has  enough  friction  that  it  is  not 
backdrivable,  then  the  aero  loads  on  the  actuator  can  be  neglected. 
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